diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/Gimpact')
42 files changed, 5335 insertions, 6009 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 diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btClipPolygon.h b/thirdparty/bullet/BulletCollision/Gimpact/btClipPolygon.h index de0a5231ba..38c23e222d 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btClipPolygon.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btClipPolygon.h @@ -27,77 +27,74 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btGeometryUtil.h" - -SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point) +SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 &plane, const btVector3 &point) { return point.dot(plane) - plane[3]; } /*! Vector blending Takes two vectors a, b, blends them together*/ -SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va,const btVector3 &vb, btScalar blend_factor) +SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va, const btVector3 &vb, btScalar blend_factor) { - vr = (1-blend_factor)*va + blend_factor*vb; + vr = (1 - blend_factor) * va + blend_factor * vb; } //! This function calcs the distance from a 3D plane SIMD_FORCE_INLINE void bt_plane_clip_polygon_collect( - const btVector3 & point0, - const btVector3 & point1, - btScalar dist0, - btScalar dist1, - btVector3 * clipped, - int & clipped_count) + const btVector3 &point0, + const btVector3 &point1, + btScalar dist0, + btScalar dist1, + btVector3 *clipped, + int &clipped_count) { - bool _prevclassif = (dist0>SIMD_EPSILON); - bool _classif = (dist1>SIMD_EPSILON); - if(_classif!=_prevclassif) + bool _prevclassif = (dist0 > SIMD_EPSILON); + bool _classif = (dist1 > SIMD_EPSILON); + if (_classif != _prevclassif) { - btScalar blendfactor = -dist0/(dist1-dist0); - bt_vec_blend(clipped[clipped_count],point0,point1,blendfactor); + btScalar blendfactor = -dist0 / (dist1 - dist0); + bt_vec_blend(clipped[clipped_count], point0, point1, blendfactor); clipped_count++; } - if(!_classif) + if (!_classif) { clipped[clipped_count] = point1; clipped_count++; } } - //! Clips a polygon by a plane /*! *\return The count of the clipped counts */ SIMD_FORCE_INLINE int bt_plane_clip_polygon( - const btVector4 & plane, - const btVector3 * polygon_points, - int polygon_point_count, - btVector3 * clipped) + const btVector4 &plane, + const btVector3 *polygon_points, + int polygon_point_count, + btVector3 *clipped) { - int clipped_count = 0; - + int clipped_count = 0; - //clip first point - btScalar firstdist = bt_distance_point_plane(plane,polygon_points[0]);; - if(!(firstdist>SIMD_EPSILON)) + //clip first point + btScalar firstdist = bt_distance_point_plane(plane, polygon_points[0]); + ; + if (!(firstdist > SIMD_EPSILON)) { clipped[clipped_count] = polygon_points[0]; clipped_count++; } btScalar olddist = firstdist; - for(int i=1;i<polygon_point_count;i++) + for (int i = 1; i < polygon_point_count; i++) { - btScalar dist = bt_distance_point_plane(plane,polygon_points[i]); + btScalar dist = bt_distance_point_plane(plane, polygon_points[i]); bt_plane_clip_polygon_collect( - polygon_points[i-1],polygon_points[i], - olddist, - dist, - clipped, - clipped_count); - + polygon_points[i - 1], polygon_points[i], + olddist, + dist, + clipped, + clipped_count); olddist = dist; } @@ -105,11 +102,11 @@ SIMD_FORCE_INLINE int bt_plane_clip_polygon( //RETURN TO FIRST point bt_plane_clip_polygon_collect( - polygon_points[polygon_point_count-1],polygon_points[0], - olddist, - firstdist, - clipped, - clipped_count); + polygon_points[polygon_point_count - 1], polygon_points[0], + olddist, + firstdist, + clipped, + clipped_count); return clipped_count; } @@ -120,18 +117,19 @@ SIMD_FORCE_INLINE int bt_plane_clip_polygon( *\return The count of the clipped counts */ SIMD_FORCE_INLINE int bt_plane_clip_triangle( - const btVector4 & plane, - const btVector3 & point0, - const btVector3 & point1, - const btVector3& point2, - btVector3 * clipped // an allocated array of 16 points at least - ) + const btVector4 &plane, + const btVector3 &point0, + const btVector3 &point1, + const btVector3 &point2, + btVector3 *clipped // an allocated array of 16 points at least +) { - int clipped_count = 0; + int clipped_count = 0; - //clip first point0 - btScalar firstdist = bt_distance_point_plane(plane,point0);; - if(!(firstdist>SIMD_EPSILON)) + //clip first point0 + btScalar firstdist = bt_distance_point_plane(plane, point0); + ; + if (!(firstdist > SIMD_EPSILON)) { clipped[clipped_count] = point0; clipped_count++; @@ -139,44 +137,37 @@ SIMD_FORCE_INLINE int bt_plane_clip_triangle( // point 1 btScalar olddist = firstdist; - btScalar dist = bt_distance_point_plane(plane,point1); + btScalar dist = bt_distance_point_plane(plane, point1); bt_plane_clip_polygon_collect( - point0,point1, - olddist, - dist, - clipped, - clipped_count); + point0, point1, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - // point 2 - dist = bt_distance_point_plane(plane,point2); + dist = bt_distance_point_plane(plane, point2); bt_plane_clip_polygon_collect( - point1,point2, - olddist, - dist, - clipped, - clipped_count); + point1, point2, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - - //RETURN TO FIRST point0 bt_plane_clip_polygon_collect( - point2,point0, - olddist, - firstdist, - clipped, - clipped_count); + point2, point0, + olddist, + firstdist, + clipped, + clipped_count); return clipped_count; } - - - - -#endif // GIM_TRI_COLLISION_H_INCLUDED +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btCompoundFromGimpact.h b/thirdparty/bullet/BulletCollision/Gimpact/btCompoundFromGimpact.h index 19f7ecddd0..ede59e8a57 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btCompoundFromGimpact.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btCompoundFromGimpact.h @@ -5,7 +5,8 @@ #include "btGImpactShape.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" -ATTRIBUTE_ALIGNED16(class) btCompoundFromGimpactShape : public btCompoundShape +ATTRIBUTE_ALIGNED16(class) +btCompoundFromGimpactShape : public btCompoundShape { public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -18,92 +19,87 @@ public: delete m_children[i].m_childShape; } } - }; struct MyCallback : public btTriangleRaycastCallback +{ + int m_ignorePart; + int m_ignoreTriangleIndex; + + MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex) + : btTriangleRaycastCallback(from, to), + m_ignorePart(ignorePart), + m_ignoreTriangleIndex(ignoreTriangleIndex) + { + } + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) + { + if (partId != m_ignorePart || triangleIndex != m_ignoreTriangleIndex) { - int m_ignorePart; - int m_ignoreTriangleIndex; - - - MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex) - :btTriangleRaycastCallback(from,to), - m_ignorePart(ignorePart), - m_ignoreTriangleIndex(ignoreTriangleIndex) - { - - } - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) - { - if (partId!=m_ignorePart || triangleIndex!=m_ignoreTriangleIndex) - { - if (hitFraction < m_hitFraction) - return hitFraction; - } - - return m_hitFraction; - } - }; - struct MyInternalTriangleIndexCallback :public btInternalTriangleIndexCallback + if (hitFraction < m_hitFraction) + return hitFraction; + } + + return m_hitFraction; + } +}; +struct MyInternalTriangleIndexCallback : public btInternalTriangleIndexCallback +{ + const btGImpactMeshShape* m_gimpactShape; + btCompoundShape* m_colShape; + btScalar m_depth; + + MyInternalTriangleIndexCallback(btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth) + : m_colShape(colShape), + m_gimpactShape(meshShape), + m_depth(depth) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + btVector3 scale = m_gimpactShape->getLocalScaling(); + btVector3 v0 = triangle[0] * scale; + btVector3 v1 = triangle[1] * scale; + btVector3 v2 = triangle[2] * scale; + + btVector3 centroid = (v0 + v1 + v2) / 3; + btVector3 normal = (v1 - v0).cross(v2 - v0); + normal.normalize(); + btVector3 rayFrom = centroid; + btVector3 rayTo = centroid - normal * m_depth; + + MyCallback cb(rayFrom, rayTo, partId, triangleIndex); + + m_gimpactShape->processAllTrianglesRay(&cb, rayFrom, rayTo); + if (cb.m_hitFraction < 1) { - const btGImpactMeshShape* m_gimpactShape; - btCompoundShape* m_colShape; - btScalar m_depth; - - MyInternalTriangleIndexCallback (btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth) - :m_colShape(colShape), - m_gimpactShape(meshShape), - m_depth(depth) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - btVector3 scale = m_gimpactShape->getLocalScaling(); - btVector3 v0=triangle[0]*scale; - btVector3 v1=triangle[1]*scale; - btVector3 v2=triangle[2]*scale; - - btVector3 centroid = (v0+v1+v2)/3; - btVector3 normal = (v1-v0).cross(v2-v0); - normal.normalize(); - btVector3 rayFrom = centroid; - btVector3 rayTo = centroid-normal*m_depth; - - MyCallback cb(rayFrom,rayTo,partId,triangleIndex); - - m_gimpactShape->processAllTrianglesRay(&cb,rayFrom, rayTo); - if (cb.m_hitFraction<1) - { - rayTo.setInterpolate3(cb.m_from,cb.m_to,cb.m_hitFraction); - //rayTo = cb.m_from; - //rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction); - //gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0)); - } - - - - btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0,v1,v2,rayTo); - btTransform ident; - ident.setIdentity(); - m_colShape->addChildShape(ident,tet); - } - }; - -btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth) + rayTo.setInterpolate3(cb.m_from, cb.m_to, cb.m_hitFraction); + //rayTo = cb.m_from; + //rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction); + //gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0)); + } + + btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0, v1, v2, rayTo); + btTransform ident; + ident.setIdentity(); + m_colShape->addChildShape(ident, tet); + } +}; + +btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth) { btCompoundShape* colShape = new btCompoundFromGimpactShape(); - - btTransform tr; - tr.setIdentity(); - - MyInternalTriangleIndexCallback cb(colShape,gimpactMesh, depth); - btVector3 aabbMin,aabbMax; - gimpactMesh->getAabb(tr,aabbMin,aabbMax); - gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb,aabbMin,aabbMax); - - return colShape; -} - -#endif //BT_COMPOUND_FROM_GIMPACT + + btTransform tr; + tr.setIdentity(); + + MyInternalTriangleIndexCallback cb(colShape, gimpactMesh, depth); + btVector3 aabbMin, aabbMax; + gimpactMesh->getAabb(tr, aabbMin, aabbMax); + gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb, aabbMin, aabbMax); + + return colShape; +} + +#endif //BT_COMPOUND_FROM_GIMPACT diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessing.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessing.cpp index eed31d839f..f2e3e18d61 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessing.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessing.cpp @@ -27,54 +27,50 @@ struct CONTACT_KEY_TOKEN unsigned int m_key; int m_value; CONTACT_KEY_TOKEN() - { - } - - CONTACT_KEY_TOKEN(unsigned int key,int token) - { - m_key = key; - m_value = token; - } - - CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken) - { - m_key = rtoken.m_key; - m_value = rtoken.m_value; - } - - inline bool operator <(const CONTACT_KEY_TOKEN& other) const + { + } + + CONTACT_KEY_TOKEN(unsigned int key, int token) + { + m_key = key; + m_value = token; + } + + CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken) + { + m_key = rtoken.m_key; + m_value = rtoken.m_value; + } + + inline bool operator<(const CONTACT_KEY_TOKEN& other) const { return (m_key < other.m_key); } - inline bool operator >(const CONTACT_KEY_TOKEN& other) const + inline bool operator>(const CONTACT_KEY_TOKEN& other) const { return (m_key > other.m_key); } - }; class CONTACT_KEY_TOKEN_COMP { - public: - - bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) const - { - return ( a < b ); - } +public: + bool operator()(const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b) const + { + return (a < b); + } }; - void btContactArray::merge_contacts( - const btContactArray & contacts, bool normal_contact_average) + const btContactArray& contacts, bool normal_contact_average) { clear(); int i; - if(contacts.size()==0) return; - + if (contacts.size() == 0) return; - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts[0]); return; @@ -86,16 +82,16 @@ void btContactArray::merge_contacts( //fill key contacts - for ( i = 0;i<contacts.size() ;i++ ) + for (i = 0; i < contacts.size(); i++) { - keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i)); + keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(), i)); } //sort keys keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP()); // Merge contacts - int coincident_count=0; + int coincident_count = 0; btVector3 coincident_normals[MAX_COINCIDENT]; unsigned int last_key = keycontacts[0].m_key; @@ -103,56 +99,56 @@ void btContactArray::merge_contacts( push_back(contacts[keycontacts[0].m_value]); - GIM_CONTACT * pcontact = &(*this)[0]; + GIM_CONTACT* pcontact = &(*this)[0]; - for( i=1;i<keycontacts.size();i++) + for (i = 1; i < keycontacts.size(); i++) { - key = keycontacts[i].m_key; - const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value]; + key = keycontacts[i].m_key; + const GIM_CONTACT* scontact = &contacts[keycontacts[i].m_value]; - if(last_key == key)//same points + if (last_key == key) //same points { //merge contact - if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) + if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth) //) { *pcontact = *scontact; - coincident_count = 0; + coincident_count = 0; } - else if(normal_contact_average) + else if (normal_contact_average) { - if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON) - { - if(coincident_count<MAX_COINCIDENT) - { - coincident_normals[coincident_count] = scontact->m_normal; - coincident_count++; - } - } + if (btFabs(pcontact->m_depth - scontact->m_depth) < CONTACT_DIFF_EPSILON) + { + if (coincident_count < MAX_COINCIDENT) + { + coincident_normals[coincident_count] = scontact->m_normal; + coincident_count++; + } + } } } else - {//add new contact + { //add new contact - if(normal_contact_average && coincident_count>0) - { - pcontact->interpolate_normals(coincident_normals,coincident_count); - coincident_count = 0; - } + if (normal_contact_average && coincident_count > 0) + { + pcontact->interpolate_normals(coincident_normals, coincident_count); + coincident_count = 0; + } - push_back(*scontact); - pcontact = &(*this)[this->size()-1]; - } + push_back(*scontact); + pcontact = &(*this)[this->size() - 1]; + } last_key = key; } } -void btContactArray::merge_contacts_unique(const btContactArray & contacts) +void btContactArray::merge_contacts_unique(const btContactArray& contacts) { clear(); - if(contacts.size()==0) return; + if (contacts.size() == 0) return; - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts[0]); return; @@ -160,14 +156,14 @@ void btContactArray::merge_contacts_unique(const btContactArray & contacts) GIM_CONTACT average_contact = contacts[0]; - for (int i=1;i<contacts.size() ;i++ ) + for (int i = 1; i < contacts.size(); i++) { average_contact.m_point += contacts[i].m_point; average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth; } //divide - btScalar divide_average = 1.0f/((btScalar)contacts.size()); + btScalar divide_average = 1.0f / ((btScalar)contacts.size()); average_contact.m_point *= divide_average; @@ -176,6 +172,4 @@ void btContactArray::merge_contacts_unique(const btContactArray & contacts) average_contact.m_depth = average_contact.m_normal.length(); average_contact.m_normal /= average_contact.m_depth; - } - diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessing.h b/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessing.h index d1027dbe67..4ff09d7cdd 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessing.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessing.h @@ -29,7 +29,7 @@ subject to the following restrictions: #include "btTriangleShapeEx.h" #include "btContactProcessingStructs.h" -class btContactArray:public btAlignedObjectArray<GIM_CONTACT> +class btContactArray : public btAlignedObjectArray<GIM_CONTACT> { public: btContactArray() @@ -38,28 +38,28 @@ public: } SIMD_FORCE_INLINE void push_contact( - const btVector3 &point,const btVector3 & normal, + const btVector3 &point, const btVector3 &normal, btScalar depth, int feature1, int feature2) { - push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) ); + push_back(GIM_CONTACT(point, normal, depth, feature1, feature2)); } SIMD_FORCE_INLINE void push_triangle_contacts( - const GIM_TRIANGLE_CONTACT & tricontact, - int feature1,int feature2) + const GIM_TRIANGLE_CONTACT &tricontact, + int feature1, int feature2) { - for(int i = 0;i<tricontact.m_point_count ;i++ ) + for (int i = 0; i < tricontact.m_point_count; i++) { push_contact( tricontact.m_points[i], tricontact.m_separating_normal, - tricontact.m_penetration_depth,feature1,feature2); + tricontact.m_penetration_depth, feature1, feature2); } } - void merge_contacts(const btContactArray & contacts, bool normal_contact_average = true); + void merge_contacts(const btContactArray &contacts, bool normal_contact_average = true); - void merge_contacts_unique(const btContactArray & contacts); + void merge_contacts_unique(const btContactArray &contacts); }; -#endif // GIM_CONTACT_H_INCLUDED +#endif // GIM_CONTACT_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessingStructs.h b/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessingStructs.h index efbc4a567a..bc8a709246 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessingStructs.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btContactProcessingStructs.h @@ -28,7 +28,6 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "btTriangleShapeEx.h" - /** Configuration var for applying interpolation of contact normals */ @@ -41,69 +40,66 @@ Configuration var for applying interpolation of contact normals class GIM_CONTACT { public: - btVector3 m_point; - btVector3 m_normal; - btScalar m_depth;//Positive value indicates interpenetration - btScalar m_distance;//Padding not for use - int m_feature1;//Face number - int m_feature2;//Face number + btVector3 m_point; + btVector3 m_normal; + btScalar m_depth; //Positive value indicates interpenetration + btScalar m_distance; //Padding not for use + int m_feature1; //Face number + int m_feature2; //Face number public: - GIM_CONTACT() - { - } - - GIM_CONTACT(const GIM_CONTACT & contact): - m_point(contact.m_point), - m_normal(contact.m_normal), - m_depth(contact.m_depth), - m_feature1(contact.m_feature1), - m_feature2(contact.m_feature2) - { - } - - GIM_CONTACT(const btVector3 &point,const btVector3 & normal, - btScalar depth, int feature1, int feature2): - m_point(point), - m_normal(normal), - m_depth(depth), - m_feature1(feature1), - m_feature2(feature2) - { - } + GIM_CONTACT() + { + } + + GIM_CONTACT(const GIM_CONTACT &contact) : m_point(contact.m_point), + m_normal(contact.m_normal), + m_depth(contact.m_depth), + m_feature1(contact.m_feature1), + m_feature2(contact.m_feature2) + { + } + + GIM_CONTACT(const btVector3 &point, const btVector3 &normal, + btScalar depth, int feature1, int feature2) : m_point(point), + m_normal(normal), + m_depth(depth), + m_feature1(feature1), + m_feature2(feature2) + { + } //! Calcs key for coord classification - SIMD_FORCE_INLINE unsigned int calc_key_contact() const - { - int _coords[] = { - (int)(m_point[0]*1000.0f+1.0f), - (int)(m_point[1]*1333.0f), - (int)(m_point[2]*2133.0f+3.0f)}; - unsigned int _hash=0; + SIMD_FORCE_INLINE unsigned int calc_key_contact() const + { + int _coords[] = { + (int)(m_point[0] * 1000.0f + 1.0f), + (int)(m_point[1] * 1333.0f), + (int)(m_point[2] * 2133.0f + 3.0f)}; + unsigned int _hash = 0; unsigned int *_uitmp = (unsigned int *)(&_coords[0]); _hash = *_uitmp; _uitmp++; - _hash += (*_uitmp)<<4; + _hash += (*_uitmp) << 4; _uitmp++; - _hash += (*_uitmp)<<8; + _hash += (*_uitmp) << 8; return _hash; - } + } - SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count) - { - btVector3 vec_sum(m_normal); - for(int i=0;i<normal_count;i++) + SIMD_FORCE_INLINE void interpolate_normals(btVector3 *normals, int normal_count) + { + btVector3 vec_sum(m_normal); + for (int i = 0; i < normal_count; i++) { vec_sum += normals[i]; } btScalar vec_sum_len = vec_sum.length2(); - if(vec_sum_len <CONTACT_DIFF_EPSILON) return; + if (vec_sum_len < CONTACT_DIFF_EPSILON) return; //GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len) - m_normal = vec_sum/btSqrt(vec_sum_len); - } - + m_normal = vec_sum / btSqrt(vec_sum_len); + } }; -#endif // BT_CONTACT_H_STRUCTS_INCLUDED +#endif // BT_CONTACT_H_STRUCTS_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp index 863233163a..bb520e061d 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp @@ -30,7 +30,6 @@ btClock g_tree_clock; float g_accum_tree_collision_time = 0; int g_count_traversing = 0; - void bt_begin_gim02_tree_time() { g_tree_clock.reset(); @@ -45,7 +44,7 @@ void bt_end_gim02_tree_time() //! Gets the average time in miliseconds of tree collisions float btGImpactBvh::getAverageTreeCollisionTime() { - if(g_count_traversing == 0) return 0; + if (g_count_traversing == 0) return 0; float avgtime = g_accum_tree_collision_time; avgtime /= (float)g_count_traversing; @@ -54,80 +53,76 @@ float btGImpactBvh::getAverageTreeCollisionTime() g_count_traversing = 0; return avgtime; -// float avgtime = g_count_traversing; -// g_count_traversing = 0; -// return avgtime; - + // float avgtime = g_count_traversing; + // g_count_traversing = 0; + // return avgtime; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING /////////////////////// btBvhTree ///////////////////////////////// int btBvhTree::_calc_splitting_axis( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) + GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex) { - int i; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); - int numIndices = endIndex-startIndex; + btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.)); + int numIndices = endIndex - startIndex; - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - means+=center; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means += center; } - means *= (btScalar(1.)/(btScalar)numIndices); + means *= (btScalar(1.) / (btScalar)numIndices); - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - btVector3 diff2 = center-means; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + btVector3 diff2 = center - means; diff2 = diff2 * diff2; variance += diff2; } - variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); + variance *= (btScalar(1.) / ((btScalar)numIndices - 1)); return variance.maxAxis(); } - int btBvhTree::_sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, + GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex, int splitAxis) { int i; - int splitIndex =startIndex; + int splitIndex = startIndex; int numIndices = endIndex - startIndex; // average of centers btScalar splitValue = 0.0f; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - for (i=startIndex;i<endIndex;i++) + btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - means+=center; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means += center; } - means *= (btScalar(1.)/(btScalar)numIndices); + means *= (btScalar(1.) / (btScalar)numIndices); splitValue = means[splitAxis]; - //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); if (center[splitAxis] > splitValue) { //swap - primitive_boxes.swap(i,splitIndex); + primitive_boxes.swap(i, splitIndex); //swapLeafNodes(i,splitIndex); splitIndex++; } @@ -142,32 +137,30 @@ int btBvhTree::_sort_and_calc_splitting_index( //bool unbalanced2 = true; //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + int rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); + btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); return splitIndex; - } - -void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex) { int curIndex = m_num_nodes; m_num_nodes++; - btAssert((endIndex-startIndex)>0); + btAssert((endIndex - startIndex) > 0); - if ((endIndex-startIndex)==1) + if ((endIndex - startIndex) == 1) { - //We have a leaf node - setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + //We have a leaf node + setNodeBound(curIndex, primitive_boxes[startIndex].m_bound); m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); return; @@ -175,47 +168,42 @@ void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startI //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. //split axis - int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); + int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex); splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex, - splitIndex//split axis - ); - + primitive_boxes, startIndex, endIndex, + splitIndex //split axis + ); //calc this node bounding box btAABB node_bound; node_bound.invalidate(); - for (int i=startIndex;i<endIndex;i++) + for (int i = startIndex; i < endIndex; i++) { node_bound.merge(primitive_boxes[i].m_bound); } - setNodeBound(curIndex,node_bound); - + setNodeBound(curIndex, node_bound); //build left branch - _build_sub_tree(primitive_boxes, startIndex, splitIndex ); - + _build_sub_tree(primitive_boxes, startIndex, splitIndex); //build right branch - _build_sub_tree(primitive_boxes, splitIndex ,endIndex); + _build_sub_tree(primitive_boxes, splitIndex, endIndex); m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex); - - } //! stackless build tree void btBvhTree::build_tree( - GIM_BVH_DATA_ARRAY & primitive_boxes) + GIM_BVH_DATA_ARRAY& primitive_boxes) { // initialize node count to 0 m_num_nodes = 0; // allocate nodes - m_node_array.resize(primitive_boxes.size()*2); + m_node_array.resize(primitive_boxes.size() * 2); _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); } @@ -225,13 +213,13 @@ void btBvhTree::build_tree( void btGImpactBvh::refit() { int nodecount = getNodeCount(); - while(nodecount--) + while (nodecount--) { - if(isLeafNode(nodecount)) + if (isLeafNode(nodecount)) { btAABB leafbox; - m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); + m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox); + setNodeBound(nodecount, leafbox); } else { @@ -243,20 +231,20 @@ void btGImpactBvh::refit() btAABB temp_box; int child_node = getLeftNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } child_node = getRightNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } - setNodeBound(nodecount,bound); + setNodeBound(nodecount, bound); } } } @@ -268,17 +256,17 @@ void btGImpactBvh::buildSet() GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); - for (int i = 0;i<primitive_boxes.size() ;i++ ) + for (int i = 0; i < primitive_boxes.size(); i++) { - m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound); - primitive_boxes[i].m_data = i; + m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound); + primitive_boxes[i].m_data = i; } m_box_tree.build_tree(primitive_boxes); } //! returns the indices of the primitives in the m_primitive_manager -bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const +bool btGImpactBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -286,7 +274,7 @@ bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & coll while (curIndex < numNodes) { btAABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data @@ -306,19 +294,17 @@ bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & coll else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } - - //! returns the indices of the primitives in the m_primitive_manager bool btGImpactBvh::rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray<int> & collided_results) const + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray<int>& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -326,16 +312,16 @@ bool btGImpactBvh::rayQuery( while (curIndex < numNodes) { btAABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) { - collided_results.push_back(getNodeData( curIndex)); + collided_results.push_back(getNodeData(curIndex)); } if (aabbOverlap || isleafnode) @@ -346,153 +332,133 @@ bool btGImpactBvh::rayQuery( else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } - SIMD_FORCE_INLINE bool _node_collision( - btGImpactBvh * boxset0, btGImpactBvh * boxset1, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, - int node0 ,int node1, bool complete_primitive_tests) + btGImpactBvh* boxset0, btGImpactBvh* boxset1, + const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, + int node0, int node1, bool complete_primitive_tests) { btAABB box0; - boxset0->getNodeBound(node0,box0); + boxset0->getNodeBound(node0, box0); btAABB box1; - boxset1->getNodeBound(node1,box1); - - return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests ); -// box1.appy_transform_trans_cache(trans_cache_1to0); -// return box0.has_collision(box1); + boxset1->getNodeBound(node1, box1); + return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests); + // box1.appy_transform_trans_cache(trans_cache_1to0); + // return box0.has_collision(box1); } - //stackless recursive collision routine static void _find_collision_pairs_recursive( - btGImpactBvh * boxset0, btGImpactBvh * boxset1, - btPairSet * collision_pairs, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, + btGImpactBvh* boxset0, btGImpactBvh* boxset1, + btPairSet* collision_pairs, + const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, int node0, int node1, bool complete_primitive_tests) { + if (_node_collision( + boxset0, boxset1, trans_cache_1to0, + node0, node1, complete_primitive_tests) == false) return; //avoid colliding internal nodes - - - if( _node_collision( - boxset0,boxset1,trans_cache_1to0, - node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes - - if(boxset0->isLeafNode(node0)) + if (boxset0->isLeafNode(node0)) { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { // collision result collision_pairs->push_pair( - boxset0->getNodeData(node0),boxset1->getNodeData(node1)); + boxset0->getNodeData(node0), boxset1->getNodeData(node1)); return; } else { - //collide left recursive _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getLeftNode(node1), false); //collide right recursive _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getRightNode(node1),false); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getRightNode(node1), false); } } else { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { - //collide left recursive _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),node1,false); - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), node1, false); //collide right recursive _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),node1,false); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), node1, false); } else { //collide left0 left1 - - _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false); //collide left0 right1 _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false); - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false); //collide right0 left1 _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false); //collide right0 right1 _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getRightNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), boxset1->getRightNode(node1), false); - }// else if node1 is not a leaf - }// else if node0 is not a leaf + } // else if node1 is not a leaf + } // else if node0 is not a leaf } - -void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0, - btGImpactBvh * boxset1, const btTransform & trans1, - btPairSet & collision_pairs) +void btGImpactBvh::find_collision(btGImpactBvh* boxset0, const btTransform& trans0, + btGImpactBvh* boxset1, const btTransform& trans1, + btPairSet& collision_pairs) { - - if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; + if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return; BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - trans_cache_1to0.calc_from_homogenic(trans0,trans1); + trans_cache_1to0.calc_from_homogenic(trans0, trans1); #ifdef TRI_COLLISION_PROFILING bt_begin_gim02_tree_time(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING _find_collision_pairs_recursive( - boxset0,boxset1, - &collision_pairs,trans_cache_1to0,0,0,true); + boxset0, boxset1, + &collision_pairs, trans_cache_1to0, 0, 0, true); #ifdef TRI_COLLISION_PROFILING bt_end_gim02_tree_time(); -#endif //TRI_COLLISION_PROFILING - +#endif //TRI_COLLISION_PROFILING } - diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvh.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvh.h index e20e03cc1d..3cd8fa24e7 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvh.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvh.h @@ -1,5 +1,5 @@ -#ifndef GIM_BOX_SET_H_INCLUDED -#define GIM_BOX_SET_H_INCLUDED +#ifndef BT_GIMPACT_BVH_H_INCLUDED +#define BT_GIMPACT_BVH_H_INCLUDED /*! \file gim_box_set.h \author Francisco Leon Najera @@ -24,7 +24,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "LinearMath/btAlignedObjectArray.h" #include "btBoxCollision.h" @@ -32,50 +31,48 @@ subject to the following restrictions: #include "btGImpactBvhStructs.h" //! A pairset array -class btPairSet: public btAlignedObjectArray<GIM_PAIR> +class btPairSet : public btAlignedObjectArray<GIM_PAIR> { public: btPairSet() { reserve(32); } - inline void push_pair(int index1,int index2) + inline void push_pair(int index1, int index2) { - push_back(GIM_PAIR(index1,index2)); + push_back(GIM_PAIR(index1, index2)); } - inline void push_pair_inv(int index1,int index2) + inline void push_pair_inv(int index1, int index2) { - push_back(GIM_PAIR(index2,index1)); + push_back(GIM_PAIR(index2, index1)); } }; -class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray<GIM_BVH_DATA> +class GIM_BVH_DATA_ARRAY : public btAlignedObjectArray<GIM_BVH_DATA> { }; - -class GIM_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<GIM_BVH_TREE_NODE> +class GIM_BVH_TREE_NODE_ARRAY : public btAlignedObjectArray<GIM_BVH_TREE_NODE> { }; - - - //! Basic Box tree structure class btBvhTree { protected: int m_num_nodes; GIM_BVH_TREE_NODE_ARRAY m_node_array; + protected: int _sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, - int startIndex, int endIndex, int splitAxis); + GIM_BVH_DATA_ARRAY& primitive_boxes, + int startIndex, int endIndex, int splitAxis); - int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + int _calc_splitting_axis(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); + + void _build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); - void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); public: btBvhTree() { @@ -84,7 +81,7 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); + void build_tree(GIM_BVH_DATA_ARRAY& primitive_boxes); SIMD_FORCE_INLINE void clearNodes() { @@ -109,25 +106,25 @@ public: return m_node_array[nodeindex].getDataIndex(); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const { bound = m_node_array[nodeindex].m_bound; } - SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound) { m_node_array[nodeindex].m_bound = bound; } SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const { - return nodeindex+1; + return nodeindex + 1; } SIMD_FORCE_INLINE int getRightNode(int nodeindex) const { - if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2; - return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex(); + if (m_node_array[nodeindex + 1].isLeafNode()) return nodeindex + 2; + return nodeindex + 1 + m_node_array[nodeindex + 1].getEscapeIndex(); } SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const @@ -135,7 +132,7 @@ public: return m_node_array[nodeindex].getEscapeIndex(); } - SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE* get_node_pointer(int index = 0) const { return &m_node_array[index]; } @@ -143,7 +140,6 @@ public: //!@} }; - //! Prototype Base class for primitive classification /*! This class is a wrapper for primitive collections. @@ -153,18 +149,16 @@ This class can manage Compound shapes and trimeshes, and if it is managing trime class btPrimitiveManagerBase { public: - virtual ~btPrimitiveManagerBase() {} //! determines if this manager consist on only triangles, which special case will be optimized virtual bool is_trimesh() const = 0; virtual int get_primitive_count() const = 0; - virtual void get_primitive_box(int prim_index ,btAABB & primbox) const = 0; + virtual void get_primitive_box(int prim_index, btAABB& primbox) const = 0; //! retrieves only the points of the triangle, and the collision margin - virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const= 0; + virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const = 0; }; - //! Structure for containing Boxes /*! This class offers an structure for managing a box tree of primitives. @@ -174,13 +168,13 @@ class btGImpactBvh { protected: btBvhTree m_box_tree; - btPrimitiveManagerBase * m_primitive_manager; + btPrimitiveManagerBase* m_primitive_manager; protected: //stackless refit void refit(); -public: +public: //! this constructor doesn't build the tree. you must call buildSet btGImpactBvh() { @@ -188,31 +182,30 @@ public: } //! this constructor doesn't build the tree. you must call buildSet - btGImpactBvh(btPrimitiveManagerBase * primitive_manager) + btGImpactBvh(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btAABB getGlobalBox() const + SIMD_FORCE_INLINE btAABB getGlobalBox() const { btAABB totalbox; getNodeBound(0, totalbox); return totalbox; } - SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) + SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const + SIMD_FORCE_INLINE btPrimitiveManagerBase* getPrimitiveManager() const { return m_primitive_manager; } - -//! node manager prototype functions -///@{ + //! node manager prototype functions + ///@{ //! this attemps to refit the box set. SIMD_FORCE_INLINE void update() @@ -224,21 +217,21 @@ public: void buildSet(); //! returns the indices of the primitives in the m_primitive_manager - bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const; + bool boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const; //! returns the indices of the primitives in the m_primitive_manager - SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box, - const btTransform & transform, btAlignedObjectArray<int> & collided_results) const + SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB& box, + const btTransform& transform, btAlignedObjectArray<int>& collided_results) const { - btAABB transbox=box; + btAABB transbox = box; transbox.appy_transform(transform); - return boxQuery(transbox,collided_results); + return boxQuery(transbox, collided_results); } //! returns the indices of the primitives in the m_primitive_manager bool rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray<int> & collided_results) const; + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray<int>& collided_results) const; //! tells if this set has hierarcht SIMD_FORCE_INLINE bool hasHierarchy() const @@ -247,7 +240,7 @@ public: } //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const + SIMD_FORCE_INLINE bool isTrimesh() const { return m_primitive_manager->is_trimesh(); } @@ -269,17 +262,16 @@ public: return m_box_tree.getNodeData(nodeindex); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const { m_box_tree.getNodeBound(nodeindex, bound); } - SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound) { m_box_tree.setNodeBound(nodeindex, bound); } - SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const { return m_box_tree.getLeftNode(nodeindex); @@ -295,24 +287,23 @@ public: return m_box_tree.getEscapeNodeIndex(nodeindex); } - SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const + SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex, btPrimitiveTriangle& triangle) const { - m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); + m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex), triangle); } - - SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE* get_node_pointer(int index = 0) const { return m_box_tree.get_node_pointer(index); } #ifdef TRI_COLLISION_PROFILING static float getAverageTreeCollisionTime(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING - static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1, - btGImpactBvh * boxset2, const btTransform & trans2, - btPairSet & collision_pairs); + static void find_collision(btGImpactBvh* boxset1, const btTransform& trans1, + btGImpactBvh* boxset2, const btTransform& trans2, + btPairSet& collision_pairs); }; -#endif // GIM_BOXPRUNING_H_INCLUDED +#endif // BT_GIMPACT_BVH_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvhStructs.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvhStructs.h index 9342a572d0..54888c6757 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvhStructs.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactBvhStructs.h @@ -24,7 +24,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "LinearMath/btAlignedObjectArray.h" #include "btBoxCollision.h" @@ -33,21 +32,22 @@ subject to the following restrictions: //! Overlapping pair struct GIM_PAIR { - int m_index1; - int m_index2; - GIM_PAIR() - {} - - GIM_PAIR(const GIM_PAIR & p) - { - m_index1 = p.m_index1; - m_index2 = p.m_index2; + int m_index1; + int m_index2; + GIM_PAIR() + { + } + + GIM_PAIR(const GIM_PAIR& p) + { + m_index1 = p.m_index1; + m_index2 = p.m_index2; } GIM_PAIR(int index1, int index2) - { - m_index1 = index1; - m_index2 = index2; + { + m_index1 = index1; + m_index2 = index2; } }; @@ -63,8 +63,10 @@ class GIM_BVH_TREE_NODE { public: btAABB m_bound; + protected: - int m_escapeIndexOrDataIndex; + int m_escapeIndexOrDataIndex; + public: GIM_BVH_TREE_NODE() { @@ -74,7 +76,7 @@ public: SIMD_FORCE_INLINE bool isLeafNode() const { //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrDataIndex>=0); + return (m_escapeIndexOrDataIndex >= 0); } SIMD_FORCE_INLINE int getEscapeIndex() const @@ -99,7 +101,6 @@ public: { m_escapeIndexOrDataIndex = index; } - }; -#endif // GIM_BOXPRUNING_H_INCLUDED +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp index 2e87475e39..3d8ab9f520 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp @@ -31,18 +31,16 @@ Concave-Concave Collision #include "btContactProcessing.h" #include "LinearMath/btQuickprof.h" - //! Class for accessing the plane equation class btPlaneShape : public btStaticPlaneShape { public: - btPlaneShape(const btVector3& v, float f) - :btStaticPlaneShape(v,f) + : btStaticPlaneShape(v, f) { } - void get_plane_equation(btVector4 &equation) + void get_plane_equation(btVector4& equation) { equation[0] = m_planeNormal[0]; equation[1] = m_planeNormal[1]; @@ -50,18 +48,16 @@ public: equation[3] = m_planeConstant; } - - void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) const + void get_plane_equation_transformed(const btTransform& trans, btVector4& equation) const { - equation[0] = trans.getBasis().getRow(0).dot(m_planeNormal); - equation[1] = trans.getBasis().getRow(1).dot(m_planeNormal); - equation[2] = trans.getBasis().getRow(2).dot(m_planeNormal); - equation[3] = trans.getOrigin().dot(m_planeNormal) + m_planeConstant; + const btVector3 normal = trans.getBasis() * m_planeNormal; + equation[0] = normal[0]; + equation[1] = normal[1]; + equation[2] = normal[2]; + equation[3] = normal.dot(trans * (m_planeConstant * m_planeNormal)); } }; - - ////////////////////////////////////////////////////////////////////////////////////////////// #ifdef TRI_COLLISION_PROFILING @@ -80,7 +76,7 @@ void bt_end_gim02_tri_time() g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds(); g_count_triangle_collision++; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING //! Retrieving shapes shapes /*! Declared here due of insuficent space on Pool allocators @@ -89,7 +85,7 @@ Declared here due of insuficent space on Pool allocators class GIM_ShapeRetriever { public: - const btGImpactShapeInterface * m_gim_shape; + const btGImpactShapeInterface* m_gim_shape; btTriangleShapeEx m_trishape; btTetrahedronShapeEx m_tetrashape; @@ -97,51 +93,50 @@ public: class ChildShapeRetriever { public: - GIM_ShapeRetriever * m_parent; - virtual const btCollisionShape * getChildShape(int index) + GIM_ShapeRetriever* m_parent; + virtual const btCollisionShape* getChildShape(int index) { return m_parent->m_gim_shape->getChildShape(index); } virtual ~ChildShapeRetriever() {} }; - class TriangleShapeRetriever:public ChildShapeRetriever + class TriangleShapeRetriever : public ChildShapeRetriever { public: - - virtual btCollisionShape * getChildShape(int index) + virtual btCollisionShape* getChildShape(int index) { - m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape); + m_parent->m_gim_shape->getBulletTriangle(index, m_parent->m_trishape); return &m_parent->m_trishape; } virtual ~TriangleShapeRetriever() {} }; - class TetraShapeRetriever:public ChildShapeRetriever + class TetraShapeRetriever : public ChildShapeRetriever { public: - - virtual btCollisionShape * getChildShape(int index) + virtual btCollisionShape* getChildShape(int index) { - m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape); + m_parent->m_gim_shape->getBulletTetrahedron(index, m_parent->m_tetrashape); return &m_parent->m_tetrashape; } }; + public: ChildShapeRetriever m_child_retriever; TriangleShapeRetriever m_tri_retriever; - TetraShapeRetriever m_tetra_retriever; - ChildShapeRetriever * m_current_retriever; + TetraShapeRetriever m_tetra_retriever; + ChildShapeRetriever* m_current_retriever; - GIM_ShapeRetriever(const btGImpactShapeInterface * gim_shape) + GIM_ShapeRetriever(const btGImpactShapeInterface* gim_shape) { m_gim_shape = gim_shape; //select retriever - if(m_gim_shape->needsRetrieveTriangles()) + if (m_gim_shape->needsRetrieveTriangles()) { m_current_retriever = &m_tri_retriever; } - else if(m_gim_shape->needsRetrieveTetrahedrons()) + else if (m_gim_shape->needsRetrieveTetrahedrons()) { m_current_retriever = &m_tetra_retriever; } @@ -153,32 +148,26 @@ public: m_current_retriever->m_parent = this; } - const btCollisionShape * getChildShape(int index) + const btCollisionShape* getChildShape(int index) { return m_current_retriever->getChildShape(index); } - - }; - - //!@} - #ifdef TRI_COLLISION_PROFILING //! Gets the average time in miliseconds of tree collisions float btGImpactCollisionAlgorithm::getAverageTreeCollisionTime() { return btGImpactBoxSet::getAverageTreeCollisionTime(); - } //! Gets the average time in miliseconds of triangle collisions float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() { - if(g_count_triangle_collision == 0) return 0; + if (g_count_triangle_collision == 0) return 0; float avgtime = g_accum_triangle_collision_time; avgtime /= (float)g_count_triangle_collision; @@ -189,12 +178,10 @@ float btGImpactCollisionAlgorithm::getAverageTriangleCollisionTime() return avgtime; } -#endif //TRI_COLLISION_PROFILING - +#endif //TRI_COLLISION_PROFILING - -btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap) +btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap) { m_manifoldPtr = NULL; m_convex_algorithm = NULL; @@ -205,77 +192,62 @@ btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm() clearCache(); } - - - - -void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btVector3 & point, - const btVector3 & normal, - btScalar distance) +void btGImpactCollisionAlgorithm::addContactPoint(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btVector3& point, + const btVector3& normal, + btScalar distance) { - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - checkManifold(body0Wrap,body1Wrap); - m_resultOut->addContactPoint(normal,point,distance); + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); + checkManifold(body0Wrap, body1Wrap); + m_resultOut->addContactPoint(normal, point, distance); } - void btGImpactCollisionAlgorithm::shape_vs_shape_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape * shape0, - const btCollisionShape * shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1) { - - { - - btCollisionAlgorithm* algor = newAlgorithm(body0Wrap,body1Wrap); + btCollisionAlgorithm* algor = newAlgorithm(body0Wrap, body1Wrap); // post : checkManifold is called - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - - algor->processCollision(body0Wrap,body1Wrap,*m_dispatchInfo,m_resultOut); - + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); + + algor->processCollision(body0Wrap, body1Wrap, *m_dispatchInfo, m_resultOut); + algor->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algor); } - } void btGImpactCollisionAlgorithm::convex_vs_convex_collision( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape* shape0, - const btCollisionShape* shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1) { + m_resultOut->setShapeIdentifiersA(m_part0, m_triface0); + m_resultOut->setShapeIdentifiersB(m_part1, m_triface1); - m_resultOut->setShapeIdentifiersA(m_part0,m_triface0); - m_resultOut->setShapeIdentifiersB(m_part1,m_triface1); - - btCollisionObjectWrapper ob0(body0Wrap,shape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0); - btCollisionObjectWrapper ob1(body1Wrap,shape1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),m_part1,m_triface1); - checkConvexAlgorithm(&ob0,&ob1); - m_convex_algorithm->processCollision(&ob0,&ob1,*m_dispatchInfo,m_resultOut); - - + btCollisionObjectWrapper ob0(body0Wrap, shape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0); + btCollisionObjectWrapper ob1(body1Wrap, shape1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), m_part1, m_triface1); + checkConvexAlgorithm(&ob0, &ob1); + m_convex_algorithm->processCollision(&ob0, &ob1, *m_dispatchInfo, m_resultOut); } - - - void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1,btPairSet & pairset) + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1, btPairSet& pairset) { - if(shape0->hasBoxSet() && shape1->hasBoxSet()) + if (shape0->hasBoxSet() && shape1->hasBoxSet()) { - btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset); + btGImpactBoxSet::find_collision(shape0->getBoxSet(), trans0, shape1->getBoxSet(), trans1, pairset); } else { @@ -283,74 +255,66 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( btAABB boxshape1; int i = shape0->getNumChildShapes(); - while(i--) + while (i--) { - shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + shape0->getChildAabb(i, trans0, boxshape0.m_min, boxshape0.m_max); int j = shape1->getNumChildShapes(); - while(j--) + while (j--) { - shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max); + shape1->getChildAabb(i, trans1, boxshape1.m_min, boxshape1.m_max); - if(boxshape1.has_collision(boxshape0)) + if (boxshape1.has_collision(boxshape0)) { - pairset.push_pair(i,j); + pairset.push_pair(i, j); } } } } - - } - void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1, - btAlignedObjectArray<int> & collided_primitives) + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, + btAlignedObjectArray<int>& collided_primitives) { - btAABB boxshape; - - if(shape0->hasBoxSet()) + if (shape0->hasBoxSet()) { btTransform trans1to0 = trans0.inverse(); trans1to0 *= trans1; - shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max); + shape1->getAabb(trans1to0, boxshape.m_min, boxshape.m_max); shape0->getBoxSet()->boxQuery(boxshape, collided_primitives); } else { - shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max); + shape1->getAabb(trans1, boxshape.m_min, boxshape.m_max); btAABB boxshape0; int i = shape0->getNumChildShapes(); - while(i--) + while (i--) { - shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + shape0->getChildAabb(i, trans0, boxshape0.m_min, boxshape0.m_max); - if(boxshape.has_collision(boxshape0)) + if (boxshape.has_collision(boxshape0)) { collided_primitives.push_back(i); } } - } - } - -void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count) +void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count) { btTriangleShapeEx tri0; btTriangleShapeEx tri1; @@ -358,27 +322,22 @@ void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectW shape0->lockChildShapes(); shape1->lockChildShapes(); - const int * pair_pointer = pairs; + const int* pair_pointer = pairs; - while(pair_count--) + while (pair_count--) { - m_triface0 = *(pair_pointer); - m_triface1 = *(pair_pointer+1); - pair_pointer+=2; - - - - shape0->getBulletTriangle(m_triface0,tri0); - shape1->getBulletTriangle(m_triface1,tri1); + m_triface1 = *(pair_pointer + 1); + pair_pointer += 2; + shape0->getBulletTriangle(m_triface0, tri0); + shape1->getBulletTriangle(m_triface1, tri1); //collide two convex shapes - if(tri0.overlap_test_conservative(tri1)) + if (tri0.overlap_test_conservative(tri1)) { - convex_vs_convex_collision(body0Wrap,body1Wrap,&tri0,&tri1); + convex_vs_convex_collision(body0Wrap, body1Wrap, &tri0, &tri1); } - } shape0->unlockChildShapes(); @@ -386,10 +345,10 @@ void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectW } void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count) { btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); @@ -401,119 +360,105 @@ void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectW shape0->lockChildShapes(); shape1->lockChildShapes(); - const int * pair_pointer = pairs; + const int* pair_pointer = pairs; - while(pair_count--) + while (pair_count--) { - m_triface0 = *(pair_pointer); - m_triface1 = *(pair_pointer+1); - pair_pointer+=2; + m_triface1 = *(pair_pointer + 1); + pair_pointer += 2; + shape0->getPrimitiveTriangle(m_triface0, ptri0); + shape1->getPrimitiveTriangle(m_triface1, ptri1); - shape0->getPrimitiveTriangle(m_triface0,ptri0); - shape1->getPrimitiveTriangle(m_triface1,ptri1); - - #ifdef TRI_COLLISION_PROFILING +#ifdef TRI_COLLISION_PROFILING bt_begin_gim02_tri_time(); - #endif +#endif ptri0.applyTransform(orgtrans0); ptri1.applyTransform(orgtrans1); - //build planes ptri0.buildTriPlane(); ptri1.buildTriPlane(); // test conservative - - - if(ptri0.overlap_test_conservative(ptri1)) + if (ptri0.overlap_test_conservative(ptri1)) { - if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data)) + if (ptri0.find_triangle_collision_clip_method(ptri1, contact_data)) { - int j = contact_data.m_point_count; - while(j--) + while (j--) { - addContactPoint(body0Wrap, body1Wrap, - contact_data.m_points[j], - contact_data.m_separating_normal, - -contact_data.m_penetration_depth); + contact_data.m_points[j], + contact_data.m_separating_normal, + -contact_data.m_penetration_depth); } } } - #ifdef TRI_COLLISION_PROFILING +#ifdef TRI_COLLISION_PROFILING bt_end_gim02_tri_time(); - #endif - +#endif } shape0->unlockChildShapes(); shape1->unlockChildShapes(); - } - void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1) { - - if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0); + const btGImpactMeshShape* meshshape0 = static_cast<const btGImpactMeshShape*>(shape0); m_part0 = meshshape0->getMeshPartCount(); - while(m_part0--) + while (m_part0--) { - gimpact_vs_gimpact(body0Wrap,body1Wrap,meshshape0->getMeshPart(m_part0),shape1); + gimpact_vs_gimpact(body0Wrap, body1Wrap, meshshape0->getMeshPart(m_part0), shape1); } return; } - if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape1 = static_cast<const btGImpactMeshShape *>(shape1); + const btGImpactMeshShape* meshshape1 = static_cast<const btGImpactMeshShape*>(shape1); m_part1 = meshshape1->getMeshPartCount(); - while(m_part1--) + while (m_part1--) { - - gimpact_vs_gimpact(body0Wrap,body1Wrap,shape0,meshshape1->getMeshPart(m_part1)); - + gimpact_vs_gimpact(body0Wrap, body1Wrap, shape0, meshshape1->getMeshPart(m_part1)); } return; } - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); btPairSet pairset; - gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset); + gimpact_vs_gimpact_find_pairs(orgtrans0, orgtrans1, shape0, shape1, pairset); - if(pairset.size()== 0) return; + if (pairset.size() == 0) return; - if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART) { - const btGImpactMeshShapePart * shapepart0 = static_cast<const btGImpactMeshShapePart * >(shape0); - const btGImpactMeshShapePart * shapepart1 = static_cast<const btGImpactMeshShapePart * >(shape1); - //specialized function - #ifdef BULLET_TRIANGLE_COLLISION - collide_gjk_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); - #else - collide_sat_triangles(body0Wrap,body1Wrap,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); - #endif + const btGImpactMeshShapePart* shapepart0 = static_cast<const btGImpactMeshShapePart*>(shape0); + const btGImpactMeshShapePart* shapepart1 = static_cast<const btGImpactMeshShapePart*>(shape1); +//specialized function +#ifdef BULLET_TRIANGLE_COLLISION + collide_gjk_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size()); +#else + collide_sat_triangles(body0Wrap, body1Wrap, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size()); +#endif return; } @@ -530,32 +475,32 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( bool child_has_transform1 = shape1->childrenHasTransform(); int i = pairset.size(); - while(i--) + while (i--) { - GIM_PAIR * pair = &pairset[i]; + GIM_PAIR* pair = &pairset[i]; m_triface0 = pair->m_index1; m_triface1 = pair->m_index2; - const btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0); - const btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1); + const btCollisionShape* colshape0 = retriever0.getChildShape(m_triface0); + const btCollisionShape* colshape1 = retriever1.getChildShape(m_triface1); btTransform tr0 = body0Wrap->getWorldTransform(); btTransform tr1 = body1Wrap->getWorldTransform(); - if(child_has_transform0) + if (child_has_transform0) { - tr0 = orgtrans0*shape0->getChildTransform(m_triface0); + tr0 = orgtrans0 * shape0->getChildTransform(m_triface0); } - if(child_has_transform1) + if (child_has_transform1) { - tr1 = orgtrans1*shape1->getChildTransform(m_triface1); + tr1 = orgtrans1 * shape1->getChildTransform(m_triface1); } - btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),tr0,m_part0,m_triface0); - btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),tr1,m_part1,m_triface1); + btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), tr0, m_part0, m_triface0); + btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), tr1, m_part1, m_triface1); //collide two convex shapes - convex_vs_convex_collision(&ob0,&ob1,colshape0,colshape1); + convex_vs_convex_collision(&ob0, &ob1, colshape0, colshape1); } shape0->unlockChildShapes(); @@ -563,159 +508,149 @@ void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( } void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1,bool swapped) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, bool swapped) { - if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE) { - const btGImpactMeshShape * meshshape0 = static_cast<const btGImpactMeshShape *>(shape0); + const btGImpactMeshShape* meshshape0 = static_cast<const btGImpactMeshShape*>(shape0); int& part = swapped ? m_part1 : m_part0; part = meshshape0->getMeshPartCount(); - while(part--) + while (part--) { - gimpact_vs_shape(body0Wrap, - body1Wrap, - meshshape0->getMeshPart(part), - shape1,swapped); - + body1Wrap, + meshshape0->getMeshPart(part), + shape1, swapped); } return; } - #ifdef GIMPACT_VS_PLANE_COLLISION - if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && +#ifdef GIMPACT_VS_PLANE_COLLISION + if (shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && shape1->getShapeType() == STATIC_PLANE_PROXYTYPE) { - const btGImpactMeshShapePart * shapepart = static_cast<const btGImpactMeshShapePart *>(shape0); - const btStaticPlaneShape * planeshape = static_cast<const btStaticPlaneShape * >(shape1); - gimpacttrimeshpart_vs_plane_collision(body0Wrap,body1Wrap,shapepart,planeshape,swapped); + const btGImpactMeshShapePart* shapepart = static_cast<const btGImpactMeshShapePart*>(shape0); + const btStaticPlaneShape* planeshape = static_cast<const btStaticPlaneShape*>(shape1); + gimpacttrimeshpart_vs_plane_collision(body0Wrap, body1Wrap, shapepart, planeshape, swapped); return; } - #endif - +#endif - - if(shape1->isCompound()) + if (shape1->isCompound()) { - const btCompoundShape * compoundshape = static_cast<const btCompoundShape *>(shape1); - gimpact_vs_compoundshape(body0Wrap,body1Wrap,shape0,compoundshape,swapped); + const btCompoundShape* compoundshape = static_cast<const btCompoundShape*>(shape1); + gimpact_vs_compoundshape(body0Wrap, body1Wrap, shape0, compoundshape, swapped); return; } - else if(shape1->isConcave()) + else if (shape1->isConcave()) { - const btConcaveShape * concaveshape = static_cast<const btConcaveShape *>(shape1); - gimpact_vs_concave(body0Wrap,body1Wrap,shape0,concaveshape,swapped); + const btConcaveShape* concaveshape = static_cast<const btConcaveShape*>(shape1); + gimpact_vs_concave(body0Wrap, body1Wrap, shape0, concaveshape, swapped); return; } - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); btAlignedObjectArray<int> collided_results; - gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results); - - if(collided_results.size() == 0) return; + gimpact_vs_shape_find_pairs(orgtrans0, orgtrans1, shape0, shape1, collided_results); + if (collided_results.size() == 0) return; shape0->lockChildShapes(); GIM_ShapeRetriever retriever0(shape0); - bool child_has_transform0 = shape0->childrenHasTransform(); - int i = collided_results.size(); - while(i--) + while (i--) { int child_index = collided_results[i]; - if(swapped) - m_triface1 = child_index; - else - m_triface0 = child_index; + if (swapped) + m_triface1 = child_index; + else + m_triface0 = child_index; - const btCollisionShape * colshape0 = retriever0.getChildShape(child_index); + const btCollisionShape* colshape0 = retriever0.getChildShape(child_index); btTransform tr0 = body0Wrap->getWorldTransform(); - if(child_has_transform0) + if (child_has_transform0) { - tr0 = orgtrans0*shape0->getChildTransform(child_index); + tr0 = orgtrans0 * shape0->getChildTransform(child_index); } - btCollisionObjectWrapper ob0(body0Wrap,colshape0,body0Wrap->getCollisionObject(),body0Wrap->getWorldTransform(),m_part0,m_triface0); + btCollisionObjectWrapper ob0(body0Wrap, colshape0, body0Wrap->getCollisionObject(), body0Wrap->getWorldTransform(), m_part0, m_triface0); const btCollisionObjectWrapper* prevObj0 = m_resultOut->getBody0Wrap(); - - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob0.getCollisionObject()) + + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob0.getCollisionObject()) { m_resultOut->setBody0Wrap(&ob0); - } else + } + else { m_resultOut->setBody1Wrap(&ob0); } //collide two shapes - if(swapped) + if (swapped) { - - shape_vs_shape_collision(body1Wrap,&ob0,shape1,colshape0); + shape_vs_shape_collision(body1Wrap, &ob0, shape1, colshape0); } else { - - shape_vs_shape_collision(&ob0,body1Wrap,colshape0,shape1); + shape_vs_shape_collision(&ob0, body1Wrap, colshape0, shape1); } m_resultOut->setBody0Wrap(prevObj0); - } shape0->unlockChildShapes(); - } void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactShapeInterface * shape0, - const btCompoundShape * shape1,bool swapped) + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCompoundShape* shape1, bool swapped) { btTransform orgtrans1 = body1Wrap->getWorldTransform(); int i = shape1->getNumChildShapes(); - while(i--) + while (i--) { + const btCollisionShape* colshape1 = shape1->getChildShape(i); + btTransform childtrans1 = orgtrans1 * shape1->getChildTransform(i); - const btCollisionShape * colshape1 = shape1->getChildShape(i); - btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i); + btCollisionObjectWrapper ob1(body1Wrap, colshape1, body1Wrap->getCollisionObject(), childtrans1, -1, i); - btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),childtrans1,-1,i); - const btCollisionObjectWrapper* tmp = 0; - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject()) { tmp = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&ob1); - } else + } + else { tmp = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&ob1); } //collide child shape gimpact_vs_shape(body0Wrap, &ob1, - shape0,colshape1,swapped); + shape0, colshape1, swapped); - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) + if (m_resultOut->getBody0Wrap()->getCollisionObject() == ob1.getCollisionObject()) { m_resultOut->setBody0Wrap(tmp); - } else + } + else { m_resultOut->setBody1Wrap(tmp); } @@ -723,27 +658,25 @@ void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObje } void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btStaticPlaneShape * shape1,bool swapped) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btStaticPlaneShape* shape1, bool swapped) { - - btTransform orgtrans0 = body0Wrap->getWorldTransform(); btTransform orgtrans1 = body1Wrap->getWorldTransform(); - const btPlaneShape * planeshape = static_cast<const btPlaneShape *>(shape1); + const btPlaneShape* planeshape = static_cast<const btPlaneShape*>(shape1); btVector4 plane; - planeshape->get_plane_equation_transformed(orgtrans1,plane); + planeshape->get_plane_equation_transformed(orgtrans1, plane); //test box against plane btAABB tribox; - shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max); + shape0->getAabb(orgtrans0, tribox.m_min, tribox.m_max); tribox.increment_margin(planeshape->getMargin()); - if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return; + if (tribox.plane_classify(plane) != BT_CONST_COLLIDE_PLANE) return; shape0->lockChildShapes(); @@ -751,28 +684,28 @@ void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( btVector3 vertex; int vi = shape0->getVertexCount(); - while(vi--) + while (vi--) { - shape0->getVertex(vi,vertex); + shape0->getVertex(vi, vertex); vertex = orgtrans0(vertex); btScalar distance = vertex.dot(plane) - plane[3] - margin; - if(distance<0.0)//add contact + if (distance < 0.0) //add contact { - if(swapped) + if (swapped) { addContactPoint(body1Wrap, body0Wrap, - vertex, - -plane, - distance); + vertex, + -plane, + distance); } else { addContactPoint(body0Wrap, body1Wrap, - vertex, - plane, - distance); + vertex, + plane, + distance); } } } @@ -780,69 +713,64 @@ void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( shape0->unlockChildShapes(); } - - - -class btGImpactTriangleCallback: public btTriangleCallback +class btGImpactTriangleCallback : public btTriangleCallback { public: - btGImpactCollisionAlgorithm * algorithm; - const btCollisionObjectWrapper * body0Wrap; - const btCollisionObjectWrapper * body1Wrap; - const btGImpactShapeInterface * gimpactshape0; + btGImpactCollisionAlgorithm* algorithm; + const btCollisionObjectWrapper* body0Wrap; + const btCollisionObjectWrapper* body1Wrap; + const btGImpactShapeInterface* gimpactshape0; bool swapped; btScalar margin; virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { - btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]); + btTriangleShapeEx tri1(triangle[0], triangle[1], triangle[2]); tri1.setMargin(margin); - if(swapped) - { - algorithm->setPart0(partId); - algorithm->setFace0(triangleIndex); - } - else - { - algorithm->setPart1(partId); - algorithm->setFace1(triangleIndex); - } - - btCollisionObjectWrapper ob1Wrap(body1Wrap,&tri1,body1Wrap->getCollisionObject(),body1Wrap->getWorldTransform(),partId,triangleIndex); - const btCollisionObjectWrapper * tmp = 0; - - if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) + if (swapped) + { + algorithm->setPart0(partId); + algorithm->setFace0(triangleIndex); + } + else + { + algorithm->setPart1(partId); + algorithm->setFace1(triangleIndex); + } + + btCollisionObjectWrapper ob1Wrap(body1Wrap, &tri1, body1Wrap->getCollisionObject(), body1Wrap->getWorldTransform(), partId, triangleIndex); + const btCollisionObjectWrapper* tmp = 0; + + if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject()) { tmp = algorithm->internalGetResultOut()->getBody0Wrap(); algorithm->internalGetResultOut()->setBody0Wrap(&ob1Wrap); - } else + } + else { tmp = algorithm->internalGetResultOut()->getBody1Wrap(); algorithm->internalGetResultOut()->setBody1Wrap(&ob1Wrap); } - + algorithm->gimpact_vs_shape( - body0Wrap,&ob1Wrap,gimpactshape0,&tri1,swapped); + body0Wrap, &ob1Wrap, gimpactshape0, &tri1, swapped); - if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) + if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject() == ob1Wrap.getCollisionObject()) { algorithm->internalGetResultOut()->setBody0Wrap(tmp); - } else + } + else { algorithm->internalGetResultOut()->setBody1Wrap(tmp); } - } }; - - - void btGImpactCollisionAlgorithm::gimpact_vs_concave( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btConcaveShape * shape1,bool swapped) + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btConcaveShape* shape1, bool swapped) { //create the callback btGImpactTriangleCallback tricallback; @@ -858,75 +786,71 @@ void btGImpactCollisionAlgorithm::gimpact_vs_concave( gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform(); - btVector3 minAABB,maxAABB; - shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB); - - shape1->processAllTriangles(&tricallback,minAABB,maxAABB); + btVector3 minAABB, maxAABB; + shape0->getAabb(gimpactInConcaveSpace, minAABB, maxAABB); + shape1->processAllTriangles(&tricallback, minAABB, maxAABB); } - - -void btGImpactCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btGImpactCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - clearCache(); + clearCache(); - m_resultOut = resultOut; + m_resultOut = resultOut; m_dispatchInfo = &dispatchInfo; - const btGImpactShapeInterface * gimpactshape0; - const btGImpactShapeInterface * gimpactshape1; + const btGImpactShapeInterface* gimpactshape0; + const btGImpactShapeInterface* gimpactshape1; - if (body0Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE) + if (body0Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape0 = static_cast<const btGImpactShapeInterface *>(body0Wrap->getCollisionShape()); + gimpactshape0 = static_cast<const btGImpactShapeInterface*>(body0Wrap->getCollisionShape()); - if( body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape()); + gimpactshape1 = static_cast<const btGImpactShapeInterface*>(body1Wrap->getCollisionShape()); - gimpact_vs_gimpact(body0Wrap,body1Wrap,gimpactshape0,gimpactshape1); + gimpact_vs_gimpact(body0Wrap, body1Wrap, gimpactshape0, gimpactshape1); } else { - gimpact_vs_shape(body0Wrap,body1Wrap,gimpactshape0,body1Wrap->getCollisionShape(),false); + gimpact_vs_shape(body0Wrap, body1Wrap, gimpactshape0, body1Wrap->getCollisionShape(), false); } - } - else if (body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) + else if (body1Wrap->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) { - gimpactshape1 = static_cast<const btGImpactShapeInterface *>(body1Wrap->getCollisionShape()); + gimpactshape1 = static_cast<const btGImpactShapeInterface*>(body1Wrap->getCollisionShape()); - gimpact_vs_shape(body1Wrap,body0Wrap,gimpactshape1,body0Wrap->getCollisionShape(),true); + gimpact_vs_shape(body1Wrap, body0Wrap, gimpactshape1, body0Wrap->getCollisionShape(), true); } -} + // Ensure that gContactProcessedCallback is called for concave shapes. + if (getLastManifold()) + { + m_resultOut->refreshContactPoints(); + } +} -btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { return 1.f; - } ///////////////////////////////////// REGISTERING ALGORITHM ////////////////////////////////////////////// - - //! Use this function for register the algorithm externally -void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) +void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher* dispatcher) { - static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf; int i; - for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { - dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf); + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE, i, &s_gimpact_cf); } - for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { - dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf); + dispatcher->registerCollisionCreateFunc(i, GIMPACT_SHAPE_PROXYTYPE, &s_gimpact_cf); } - } diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h index 3e5675f729..a368c8a0c0 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -42,7 +42,6 @@ class btDispatcher; #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - //! Collision Algorithm for GImpact Shapes /*! For register this algorithm in Bullet, proceed as following: @@ -54,36 +53,35 @@ btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher); class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm { protected: - btCollisionAlgorithm * m_convex_algorithm; - btPersistentManifold * m_manifoldPtr; + btCollisionAlgorithm* m_convex_algorithm; + btPersistentManifold* m_manifoldPtr; btManifoldResult* m_resultOut; - const btDispatcherInfo * m_dispatchInfo; + const btDispatcherInfo* m_dispatchInfo; int m_triface0; int m_part0; int m_triface1; int m_part1; - //! Creates a new contact point - SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(const btCollisionObject* body0,const btCollisionObject* body1) + SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(const btCollisionObject* body0, const btCollisionObject* body1) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_manifoldPtr = m_dispatcher->getNewManifold(body0, body1); return m_manifoldPtr; } SIMD_FORCE_INLINE void destroyConvexAlgorithm() { - if(m_convex_algorithm) + if (m_convex_algorithm) { m_convex_algorithm->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm); + m_dispatcher->freeCollisionAlgorithm(m_convex_algorithm); m_convex_algorithm = NULL; } } SIMD_FORCE_INLINE void destroyContactManifolds() { - if(m_manifoldPtr == NULL) return; + if (m_manifoldPtr == NULL) return; m_dispatcher->releaseManifold(m_manifoldPtr); m_manifoldPtr = NULL; } @@ -104,207 +102,187 @@ protected: return m_manifoldPtr; } - // Call before process collision - SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - if(getLastManifold() == 0) + if (getLastManifold() == 0) { - newContactManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + newContactManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); } m_resultOut->setPersistentManifold(getLastManifold()); } // Call before process collision - SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + SIMD_FORCE_INLINE btCollisionAlgorithm* newAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - checkManifold(body0Wrap,body1Wrap); + checkManifold(body0Wrap, body1Wrap); - btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm( - body0Wrap,body1Wrap,getLastManifold(), BT_CONTACT_POINT_ALGORITHMS); - return convex_algorithm ; + btCollisionAlgorithm* convex_algorithm = m_dispatcher->findAlgorithm( + body0Wrap, body1Wrap, getLastManifold(), BT_CONTACT_POINT_ALGORITHMS); + return convex_algorithm; } // Call before process collision - SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - if(m_convex_algorithm) return; - m_convex_algorithm = newAlgorithm(body0Wrap,body1Wrap); + if (m_convex_algorithm) return; + m_convex_algorithm = newAlgorithm(body0Wrap, body1Wrap); } + void addContactPoint(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btVector3& point, + const btVector3& normal, + btScalar distance); - - - void addContactPoint(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btVector3 & point, - const btVector3 & normal, - btScalar distance); - -//! Collision routines -//!@{ + //! Collision routines + //!@{ void collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count); + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count); void collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count); - - - + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btGImpactMeshShapePart* shape1, + const int* pairs, int pair_count); void shape_vs_shape_collision( - const btCollisionObjectWrapper* body0, - const btCollisionObjectWrapper* body1, - const btCollisionShape * shape0, - const btCollisionShape * shape1); + const btCollisionObjectWrapper* body0, + const btCollisionObjectWrapper* body1, + const btCollisionShape* shape0, + const btCollisionShape* shape1); void convex_vs_convex_collision(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape* shape0, - const btCollisionShape* shape1); - - + const btCollisionObjectWrapper* body1Wrap, + const btCollisionShape* shape0, + const btCollisionShape* shape1); void gimpact_vs_gimpact_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1,btPairSet & pairset); + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1, btPairSet& pairset); void gimpact_vs_shape_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1, - btAlignedObjectArray<int> & collided_primitives); - + const btTransform& trans0, + const btTransform& trans1, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, + btAlignedObjectArray<int>& collided_primitives); void gimpacttrimeshpart_vs_plane_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btStaticPlaneShape * shape1,bool swapped); - + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactMeshShapePart* shape0, + const btStaticPlaneShape* shape1, bool swapped); public: - - btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btGImpactCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr) manifoldArray.push_back(m_manifoldPtr); } - btManifoldResult* internalGetResultOut() + btManifoldResult* internalGetResultOut() { return m_resultOut; } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm)); - return new(mem) btGImpactCollisionAlgorithm(ci,body0Wrap,body1Wrap); + return new (mem) btGImpactCollisionAlgorithm(ci, body0Wrap, body1Wrap); } }; //! Use this function for register the algorithm externally - static void registerAlgorithm(btCollisionDispatcher * dispatcher); + static void registerAlgorithm(btCollisionDispatcher* dispatcher); #ifdef TRI_COLLISION_PROFILING //! Gets the average time in miliseconds of tree collisions static float getAverageTreeCollisionTime(); //! Gets the average time in miliseconds of triangle collisions static float getAverageTriangleCollisionTime(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING //! Collides two gimpact shapes /*! \pre shape0 and shape1 couldn't be btGImpactMeshShape objects */ - void gimpact_vs_gimpact(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1); + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btGImpactShapeInterface* shape1); void gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1,bool swapped); + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCollisionShape* shape1, bool swapped); - void gimpact_vs_compoundshape(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btCompoundShape * shape1,bool swapped); + void gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btCompoundShape* shape1, bool swapped); void gimpact_vs_concave( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btConcaveShape * shape1,bool swapped); - - - - - /// Accessor/Mutator pairs for Part and triangleID - void setFace0(int value) - { - m_triface0 = value; - } - int getFace0() - { - return m_triface0; - } - void setFace1(int value) - { - m_triface1 = value; - } - int getFace1() - { - return m_triface1; - } - void setPart0(int value) - { - m_part0 = value; - } - int getPart0() - { - return m_part0; - } - void setPart1(int value) - { - m_part1 = value; - } - int getPart1() - { - return m_part1; - } + const btCollisionObjectWrapper* body0Wrap, + const btCollisionObjectWrapper* body1Wrap, + const btGImpactShapeInterface* shape0, + const btConcaveShape* shape1, bool swapped); + /// Accessor/Mutator pairs for Part and triangleID + void setFace0(int value) + { + m_triface0 = value; + } + int getFace0() + { + return m_triface0; + } + void setFace1(int value) + { + m_triface1 = value; + } + int getFace1() + { + return m_triface1; + } + void setPart0(int value) + { + m_part0 = value; + } + int getPart0() + { + return m_part0; + } + void setPart1(int value) + { + m_part1 = value; + } + int getPart1() + { + return m_part1; + } }; - //algorithm details //#define BULLET_TRIANGLE_COLLISION 1 #define GIMPACT_VS_PLANE_COLLISION 1 - - -#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactMassUtil.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactMassUtil.h index 2543aefcfc..1cde46ed8b 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactMassUtil.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactMassUtil.h @@ -21,40 +21,36 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GIMPACT_MASS_UTIL_H #define GIMPACT_MASS_UTIL_H #include "LinearMath/btTransform.h" - - SIMD_FORCE_INLINE btVector3 gim_inertia_add_transformed( - const btVector3 & source_inertia, const btVector3 & added_inertia, const btTransform & transform) + const btVector3& source_inertia, const btVector3& added_inertia, const btTransform& transform) { - btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose(); + btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose(); btScalar x2 = transform.getOrigin()[0]; - x2*= x2; + x2 *= x2; btScalar y2 = transform.getOrigin()[1]; - y2*= y2; + y2 *= y2; btScalar z2 = transform.getOrigin()[2]; - z2*= z2; + z2 *= z2; - btScalar ix = rotatedTensor[0][0]*(y2+z2); - btScalar iy = rotatedTensor[1][1]*(x2+z2); - btScalar iz = rotatedTensor[2][2]*(x2+y2); + btScalar ix = rotatedTensor[0][0] * (y2 + z2); + btScalar iy = rotatedTensor[1][1] * (x2 + z2); + btScalar iz = rotatedTensor[2][2] * (x2 + y2); - return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz); + return btVector3(source_inertia[0] + ix, source_inertia[1] + iy, source_inertia[2] + iz); } -SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3 & point, btScalar mass) +SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3& point, btScalar mass) { - btScalar x2 = point[0]*point[0]; - btScalar y2 = point[1]*point[1]; - btScalar z2 = point[2]*point[2]; - return btVector3(mass*(y2+z2),mass*(x2+z2),mass*(x2+y2)); + btScalar x2 = point[0] * point[0]; + btScalar y2 = point[1] * point[1]; + btScalar z2 = point[2] * point[2]; + return btVector3(mass * (y2 + z2), mass * (x2 + z2), mass * (x2 + y2)); } - -#endif //GIMPACT_MESH_SHAPE_H +#endif //GIMPACT_MESH_SHAPE_H diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp index 4528758c37..b81fc97044 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp @@ -27,11 +27,9 @@ subject to the following restrictions: #ifdef TRI_COLLISION_PROFILING btClock g_q_tree_clock; - float g_q_accum_tree_collision_time = 0; int g_q_count_traversing = 0; - void bt_begin_gim02_q_tree_time() { g_q_tree_clock.reset(); @@ -43,11 +41,10 @@ void bt_end_gim02_q_tree_time() g_q_count_traversing++; } - //! Gets the average time in miliseconds of tree collisions float btGImpactQuantizedBvh::getAverageTreeCollisionTime() { - if(g_q_count_traversing == 0) return 0; + if (g_q_count_traversing == 0) return 0; float avgtime = g_q_accum_tree_collision_time; avgtime /= (float)g_q_count_traversing; @@ -56,99 +53,92 @@ float btGImpactQuantizedBvh::getAverageTreeCollisionTime() g_q_count_traversing = 0; return avgtime; -// float avgtime = g_q_count_traversing; -// g_q_count_traversing = 0; -// return avgtime; - + // float avgtime = g_q_count_traversing; + // g_q_count_traversing = 0; + // return avgtime; } -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING /////////////////////// btQuantizedBvhTree ///////////////////////////////// void btQuantizedBvhTree::calc_quantization( - GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) + GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin) { //calc globa box btAABB global_bound; global_bound.invalidate(); - for (int i=0;i<primitive_boxes.size() ;i++ ) + for (int i = 0; i < primitive_boxes.size(); i++) { global_bound.merge(primitive_boxes[i].m_bound); } bt_calc_quantization_parameters( - m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin); - + m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization, global_bound.m_min, global_bound.m_max, boundMargin); } - - int btQuantizedBvhTree::_calc_splitting_axis( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) + GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex) { - int i; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); - int numIndices = endIndex-startIndex; + btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.)); + int numIndices = endIndex - startIndex; - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - means+=center; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means += center; } - means *= (btScalar(1.)/(btScalar)numIndices); + means *= (btScalar(1.) / (btScalar)numIndices); - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - btVector3 diff2 = center-means; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + btVector3 diff2 = center - means; diff2 = diff2 * diff2; variance += diff2; } - variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); + variance *= (btScalar(1.) / ((btScalar)numIndices - 1)); return variance.maxAxis(); } - int btQuantizedBvhTree::_sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, + GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex, int splitAxis) { int i; - int splitIndex =startIndex; + int splitIndex = startIndex; int numIndices = endIndex - startIndex; // average of centers btScalar splitValue = 0.0f; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - for (i=startIndex;i<endIndex;i++) + btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - means+=center; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means += center; } - means *= (btScalar(1.)/(btScalar)numIndices); + means *= (btScalar(1.) / (btScalar)numIndices); splitValue = means[splitAxis]; - //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); if (center[splitAxis] > splitValue) { //swap - primitive_boxes.swap(i,splitIndex); + primitive_boxes.swap(i, splitIndex); //swapLeafNodes(i,splitIndex); splitIndex++; } @@ -163,32 +153,30 @@ int btQuantizedBvhTree::_sort_and_calc_splitting_index( //bool unbalanced2 = true; //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + int rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); + btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); return splitIndex; - } - -void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex) { int curIndex = m_num_nodes; m_num_nodes++; - btAssert((endIndex-startIndex)>0); + btAssert((endIndex - startIndex) > 0); - if ((endIndex-startIndex)==1) + if ((endIndex - startIndex) == 1) { - //We have a leaf node - setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + //We have a leaf node + setNodeBound(curIndex, primitive_boxes[startIndex].m_bound); m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); return; @@ -196,48 +184,43 @@ void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, i //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. //split axis - int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); + int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex); splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex, - splitIndex//split axis - ); - + primitive_boxes, startIndex, endIndex, + splitIndex //split axis + ); //calc this node bounding box btAABB node_bound; node_bound.invalidate(); - for (int i=startIndex;i<endIndex;i++) + for (int i = startIndex; i < endIndex; i++) { node_bound.merge(primitive_boxes[i].m_bound); } - setNodeBound(curIndex,node_bound); - + setNodeBound(curIndex, node_bound); //build left branch - _build_sub_tree(primitive_boxes, startIndex, splitIndex ); - + _build_sub_tree(primitive_boxes, startIndex, splitIndex); //build right branch - _build_sub_tree(primitive_boxes, splitIndex ,endIndex); + _build_sub_tree(primitive_boxes, splitIndex, endIndex); m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex); - - } //! stackless build tree void btQuantizedBvhTree::build_tree( - GIM_BVH_DATA_ARRAY & primitive_boxes) + GIM_BVH_DATA_ARRAY& primitive_boxes) { calc_quantization(primitive_boxes); // initialize node count to 0 m_num_nodes = 0; // allocate nodes - m_node_array.resize(primitive_boxes.size()*2); + m_node_array.resize(primitive_boxes.size() * 2); _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); } @@ -247,13 +230,13 @@ void btQuantizedBvhTree::build_tree( void btGImpactQuantizedBvh::refit() { int nodecount = getNodeCount(); - while(nodecount--) + while (nodecount--) { - if(isLeafNode(nodecount)) + if (isLeafNode(nodecount)) { btAABB leafbox; - m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); + m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox); + setNodeBound(nodecount, leafbox); } else { @@ -265,20 +248,20 @@ void btGImpactQuantizedBvh::refit() btAABB temp_box; int child_node = getLeftNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } child_node = getRightNode(nodecount); - if(child_node) + if (child_node) { - getNodeBound(child_node,temp_box); + getNodeBound(child_node, temp_box); bound.merge(temp_box); } - setNodeBound(nodecount,bound); + setNodeBound(nodecount, bound); } } } @@ -290,17 +273,17 @@ void btGImpactQuantizedBvh::buildSet() GIM_BVH_DATA_ARRAY primitive_boxes; primitive_boxes.resize(m_primitive_manager->get_primitive_count()); - for (int i = 0;i<primitive_boxes.size() ;i++ ) + for (int i = 0; i < primitive_boxes.size(); i++) { - m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound); - primitive_boxes[i].m_data = i; + m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound); + primitive_boxes[i].m_data = i; } m_box_tree.build_tree(primitive_boxes); } //! returns the indices of the primitives in the m_primitive_manager -bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const +bool btGImpactQuantizedBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -310,16 +293,14 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in unsigned short quantizedMin[3]; unsigned short quantizedMax[3]; - m_box_tree.quantizePoint(quantizedMin,box.m_min); - m_box_tree.quantizePoint(quantizedMax,box.m_max); - + m_box_tree.quantizePoint(quantizedMin, box.m_min); + m_box_tree.quantizePoint(quantizedMax, box.m_max); while (curIndex < numNodes) { - //catch bugs in tree data - bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax); + bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin, quantizedMax); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) @@ -335,19 +316,17 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } - - //! returns the indices of the primitives in the m_primitive_manager bool btGImpactQuantizedBvh::rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray<int> & collided_results) const + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray<int>& collided_results) const { int curIndex = 0; int numNodes = getNodeCount(); @@ -355,16 +334,16 @@ bool btGImpactQuantizedBvh::rayQuery( while (curIndex < numNodes) { btAABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) { - collided_results.push_back(getNodeData( curIndex)); + collided_results.push_back(getNodeData(curIndex)); } if (aabbOverlap || isleafnode) @@ -375,154 +354,133 @@ bool btGImpactQuantizedBvh::rayQuery( else { //skip node - curIndex+= getEscapeNodeIndex(curIndex); + curIndex += getEscapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } - SIMD_FORCE_INLINE bool _quantized_node_collision( - const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, - int node0 ,int node1, bool complete_primitive_tests) + const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1, + const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, + int node0, int node1, bool complete_primitive_tests) { btAABB box0; - boxset0->getNodeBound(node0,box0); + boxset0->getNodeBound(node0, box0); btAABB box1; - boxset1->getNodeBound(node1,box1); - - return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests ); -// box1.appy_transform_trans_cache(trans_cache_1to0); -// return box0.has_collision(box1); + boxset1->getNodeBound(node1, box1); + return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests); + // box1.appy_transform_trans_cache(trans_cache_1to0); + // return box0.has_collision(box1); } - //stackless recursive collision routine static void _find_quantized_collision_pairs_recursive( - const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1, - btPairSet * collision_pairs, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, + const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1, + btPairSet* collision_pairs, + const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0, int node0, int node1, bool complete_primitive_tests) { + if (_quantized_node_collision( + boxset0, boxset1, trans_cache_1to0, + node0, node1, complete_primitive_tests) == false) return; //avoid colliding internal nodes - - - if( _quantized_node_collision( - boxset0,boxset1,trans_cache_1to0, - node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes - - if(boxset0->isLeafNode(node0)) + if (boxset0->isLeafNode(node0)) { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { // collision result collision_pairs->push_pair( - boxset0->getNodeData(node0),boxset1->getNodeData(node1)); + boxset0->getNodeData(node0), boxset1->getNodeData(node1)); return; } else { - //collide left recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getLeftNode(node1), false); //collide right recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getRightNode(node1),false); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + node0, boxset1->getRightNode(node1), false); } } else { - if(boxset1->isLeafNode(node1)) + if (boxset1->isLeafNode(node1)) { - //collide left recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),node1,false); - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), node1, false); //collide right recursive _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),node1,false); - - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), node1, false); } else { //collide left0 left1 - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false); //collide left0 right1 _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false); - + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false); //collide right0 left1 _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false); //collide right0 right1 _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getRightNode(node1),false); + boxset0, boxset1, + collision_pairs, trans_cache_1to0, + boxset0->getRightNode(node0), boxset1->getRightNode(node1), false); - }// else if node1 is not a leaf - }// else if node0 is not a leaf + } // else if node1 is not a leaf + } // else if node0 is not a leaf } - -void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0, - const btGImpactQuantizedBvh * boxset1, const btTransform & trans1, - btPairSet & collision_pairs) +void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh* boxset0, const btTransform& trans0, + const btGImpactQuantizedBvh* boxset1, const btTransform& trans1, + btPairSet& collision_pairs) { - - if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; + if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return; BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - trans_cache_1to0.calc_from_homogenic(trans0,trans1); + trans_cache_1to0.calc_from_homogenic(trans0, trans1); #ifdef TRI_COLLISION_PROFILING bt_begin_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - &collision_pairs,trans_cache_1to0,0,0,true); + boxset0, boxset1, + &collision_pairs, trans_cache_1to0, 0, 0, true); #ifdef TRI_COLLISION_PROFILING bt_end_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING - +#endif //TRI_COLLISION_PROFILING } - - diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.h index 42e5520fc0..b231c1e832 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.h @@ -28,13 +28,10 @@ subject to the following restrictions: #include "btQuantization.h" #include "btGImpactQuantizedBvhStructs.h" -class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE> +class GIM_QUANTIZED_BVH_NODE_ARRAY : public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE> { }; - - - //! Basic Box tree structure class btQuantizedBvhTree { @@ -43,16 +40,18 @@ protected: GIM_QUANTIZED_BVH_NODE_ARRAY m_node_array; btAABB m_global_bound; btVector3 m_bvhQuantization; + protected: - void calc_quantization(GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) ); + void calc_quantization(GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin = btScalar(1.0)); int _sort_and_calc_splitting_index( - GIM_BVH_DATA_ARRAY & primitive_boxes, - int startIndex, int endIndex, int splitAxis); + GIM_BVH_DATA_ARRAY& primitive_boxes, + int startIndex, int endIndex, int splitAxis); - int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + int _calc_splitting_axis(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); + + void _build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex); - void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); public: btQuantizedBvhTree() { @@ -61,20 +60,19 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); + void build_tree(GIM_BVH_DATA_ARRAY& primitive_boxes); SIMD_FORCE_INLINE void quantizePoint( - unsigned short * quantizedpoint, const btVector3 & point) const + unsigned short* quantizedpoint, const btVector3& point) const { - bt_quantize_clamp(quantizedpoint,point,m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization); + bt_quantize_clamp(quantizedpoint, point, m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization); } - SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( int node_index, - unsigned short * quantizedMin,unsigned short * quantizedMax) const + unsigned short* quantizedMin, unsigned short* quantizedMax) const { - return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax); + return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin, quantizedMax); } SIMD_FORCE_INLINE void clearNodes() @@ -100,41 +98,41 @@ public: return m_node_array[nodeindex].getDataIndex(); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const { bound.m_min = bt_unquantize( m_node_array[nodeindex].m_quantizedAabbMin, - m_global_bound.m_min,m_bvhQuantization); + m_global_bound.m_min, m_bvhQuantization); bound.m_max = bt_unquantize( m_node_array[nodeindex].m_quantizedAabbMax, - m_global_bound.m_min,m_bvhQuantization); + m_global_bound.m_min, m_bvhQuantization); } - SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound) { - bt_quantize_clamp( m_node_array[nodeindex].m_quantizedAabbMin, - bound.m_min, - m_global_bound.m_min, - m_global_bound.m_max, - m_bvhQuantization); + bt_quantize_clamp(m_node_array[nodeindex].m_quantizedAabbMin, + bound.m_min, + m_global_bound.m_min, + m_global_bound.m_max, + m_bvhQuantization); - bt_quantize_clamp( m_node_array[nodeindex].m_quantizedAabbMax, - bound.m_max, - m_global_bound.m_min, - m_global_bound.m_max, - m_bvhQuantization); + bt_quantize_clamp(m_node_array[nodeindex].m_quantizedAabbMax, + bound.m_max, + m_global_bound.m_min, + m_global_bound.m_max, + m_bvhQuantization); } SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const { - return nodeindex+1; + return nodeindex + 1; } SIMD_FORCE_INLINE int getRightNode(int nodeindex) const { - if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2; - return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex(); + if (m_node_array[nodeindex + 1].isLeafNode()) return nodeindex + 2; + return nodeindex + 1 + m_node_array[nodeindex + 1].getEscapeIndex(); } SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const @@ -142,7 +140,7 @@ public: return m_node_array[nodeindex].getEscapeIndex(); } - SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE* get_node_pointer(int index = 0) const { return &m_node_array[index]; } @@ -150,8 +148,6 @@ public: //!@} }; - - //! Structure for containing Boxes /*! This class offers an structure for managing a box tree of primitives. @@ -161,13 +157,13 @@ class btGImpactQuantizedBvh { protected: btQuantizedBvhTree m_box_tree; - btPrimitiveManagerBase * m_primitive_manager; + btPrimitiveManagerBase* m_primitive_manager; protected: //stackless refit void refit(); -public: +public: //! this constructor doesn't build the tree. you must call buildSet btGImpactQuantizedBvh() { @@ -175,31 +171,30 @@ public: } //! this constructor doesn't build the tree. you must call buildSet - btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager) + btGImpactQuantizedBvh(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btAABB getGlobalBox() const + SIMD_FORCE_INLINE btAABB getGlobalBox() const { btAABB totalbox; getNodeBound(0, totalbox); return totalbox; } - SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) + SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase* primitive_manager) { m_primitive_manager = primitive_manager; } - SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const + SIMD_FORCE_INLINE btPrimitiveManagerBase* getPrimitiveManager() const { return m_primitive_manager; } - -//! node manager prototype functions -///@{ + //! node manager prototype functions + ///@{ //! this attemps to refit the box set. SIMD_FORCE_INLINE void update() @@ -211,21 +206,21 @@ public: void buildSet(); //! returns the indices of the primitives in the m_primitive_manager - bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const; + bool boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const; //! returns the indices of the primitives in the m_primitive_manager - SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box, - const btTransform & transform, btAlignedObjectArray<int> & collided_results) const + SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB& box, + const btTransform& transform, btAlignedObjectArray<int>& collided_results) const { - btAABB transbox=box; + btAABB transbox = box; transbox.appy_transform(transform); - return boxQuery(transbox,collided_results); + return boxQuery(transbox, collided_results); } //! returns the indices of the primitives in the m_primitive_manager bool rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray<int> & collided_results) const; + const btVector3& ray_dir, const btVector3& ray_origin, + btAlignedObjectArray<int>& collided_results) const; //! tells if this set has hierarcht SIMD_FORCE_INLINE bool hasHierarchy() const @@ -234,7 +229,7 @@ public: } //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const + SIMD_FORCE_INLINE bool isTrimesh() const { return m_primitive_manager->is_trimesh(); } @@ -256,17 +251,16 @@ public: return m_box_tree.getNodeData(nodeindex); } - SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const { m_box_tree.getNodeBound(nodeindex, bound); } - SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound) + SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound) { m_box_tree.setNodeBound(nodeindex, bound); } - SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const { return m_box_tree.getLeftNode(nodeindex); @@ -282,24 +276,23 @@ public: return m_box_tree.getEscapeNodeIndex(nodeindex); } - SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const + SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex, btPrimitiveTriangle& triangle) const { - m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); + m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex), triangle); } - - SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const + SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE* get_node_pointer(int index = 0) const { return m_box_tree.get_node_pointer(index); } #ifdef TRI_COLLISION_PROFILING static float getAverageTreeCollisionTime(); -#endif //TRI_COLLISION_PROFILING +#endif //TRI_COLLISION_PROFILING - static void find_collision(const btGImpactQuantizedBvh * boxset1, const btTransform & trans1, - const btGImpactQuantizedBvh * boxset2, const btTransform & trans2, - btPairSet & collision_pairs); + static void find_collision(const btGImpactQuantizedBvh* boxset1, const btTransform& trans1, + const btGImpactQuantizedBvh* boxset2, const btTransform& trans2, + btPairSet& collision_pairs); }; -#endif // GIM_BOXPRUNING_H_INCLUDED +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h index 7dd5a1b9d0..bd50cb5b87 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h @@ -29,13 +29,14 @@ subject to the following restrictions: ///btQuantizedBvhNode is a compressed aabb node, 16 bytes. ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). -ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE +ATTRIBUTE_ALIGNED16(struct) +BT_QUANTIZED_BVH_NODE { //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; //4 bytes - int m_escapeIndexOrDataIndex; + int m_escapeIndexOrDataIndex; BT_QUANTIZED_BVH_NODE() { @@ -45,7 +46,7 @@ ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE SIMD_FORCE_INLINE bool isLeafNode() const { //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrDataIndex>=0); + return (m_escapeIndexOrDataIndex >= 0); } SIMD_FORCE_INLINE int getEscapeIndex() const @@ -72,20 +73,19 @@ ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE } SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( - unsigned short * quantizedMin,unsigned short * quantizedMax) const + unsigned short* quantizedMin, unsigned short* quantizedMax) const { - if(m_quantizedAabbMin[0] > quantizedMax[0] || - m_quantizedAabbMax[0] < quantizedMin[0] || - m_quantizedAabbMin[1] > quantizedMax[1] || - m_quantizedAabbMax[1] < quantizedMin[1] || - m_quantizedAabbMin[2] > quantizedMax[2] || - m_quantizedAabbMax[2] < quantizedMin[2]) + if (m_quantizedAabbMin[0] > quantizedMax[0] || + m_quantizedAabbMax[0] < quantizedMin[0] || + m_quantizedAabbMin[1] > quantizedMax[1] || + m_quantizedAabbMax[1] < quantizedMin[1] || + m_quantizedAabbMin[2] > quantizedMax[2] || + m_quantizedAabbMax[2] < quantizedMin[2]) { return false; } return true; } - }; -#endif // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED +#endif // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.cpp index 30c85e3fff..34c229a3ab 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.cpp @@ -18,178 +18,169 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btGImpactShape.h" #include "btGImpactMassUtil.h" - -btGImpactMeshShapePart::btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part ) +btGImpactMeshShapePart::btGImpactMeshShapePart(btStridingMeshInterface* meshInterface, int part) { - // moved from .h to .cpp because of conditional compilation - // (The setting of BT_THREADSAFE may differ between various cpp files, so it is best to - // avoid using it in h files) - m_primitive_manager.m_meshInterface = meshInterface; - m_primitive_manager.m_part = part; - m_box_set.setPrimitiveManager( &m_primitive_manager ); + // moved from .h to .cpp because of conditional compilation + // (The setting of BT_THREADSAFE may differ between various cpp files, so it is best to + // avoid using it in h files) + m_primitive_manager.m_meshInterface = meshInterface; + m_primitive_manager.m_part = part; + m_box_set.setPrimitiveManager(&m_primitive_manager); #if BT_THREADSAFE - // If threadsafe is requested, this object uses a different lock/unlock - // model with the btStridingMeshInterface -- lock once when the object is constructed - // and unlock once in the destructor. - // The other way of locking and unlocking for each collision check in the narrowphase - // is not threadsafe. Note these are not thread-locks, they are calls to the meshInterface's - // getLockedReadOnlyVertexIndexBase virtual function, which by default just returns a couple of - // pointers. In theory a client could override the lock function to do all sorts of - // things like reading data from GPU memory, or decompressing data on the fly, but such things - // do not seem all that likely or useful, given the performance cost. - m_primitive_manager.lock(); + // If threadsafe is requested, this object uses a different lock/unlock + // model with the btStridingMeshInterface -- lock once when the object is constructed + // and unlock once in the destructor. + // The other way of locking and unlocking for each collision check in the narrowphase + // is not threadsafe. Note these are not thread-locks, they are calls to the meshInterface's + // getLockedReadOnlyVertexIndexBase virtual function, which by default just returns a couple of + // pointers. In theory a client could override the lock function to do all sorts of + // things like reading data from GPU memory, or decompressing data on the fly, but such things + // do not seem all that likely or useful, given the performance cost. + m_primitive_manager.lock(); #endif } btGImpactMeshShapePart::~btGImpactMeshShapePart() { - // moved from .h to .cpp because of conditional compilation + // moved from .h to .cpp because of conditional compilation #if BT_THREADSAFE - m_primitive_manager.unlock(); + m_primitive_manager.unlock(); #endif } void btGImpactMeshShapePart::lockChildShapes() const { - // moved from .h to .cpp because of conditional compilation -#if ! BT_THREADSAFE - // called in the narrowphase -- not threadsafe! - void * dummy = (void*) ( m_box_set.getPrimitiveManager() ); - TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>( dummy ); - dummymanager->lock(); + // moved from .h to .cpp because of conditional compilation +#if !BT_THREADSAFE + // called in the narrowphase -- not threadsafe! + void* dummy = (void*)(m_box_set.getPrimitiveManager()); + TrimeshPrimitiveManager* dummymanager = static_cast<TrimeshPrimitiveManager*>(dummy); + dummymanager->lock(); #endif } -void btGImpactMeshShapePart::unlockChildShapes() const +void btGImpactMeshShapePart::unlockChildShapes() const { - // moved from .h to .cpp because of conditional compilation -#if ! BT_THREADSAFE - // called in the narrowphase -- not threadsafe! - void * dummy = (void*) ( m_box_set.getPrimitiveManager() ); - TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>( dummy ); - dummymanager->unlock(); + // moved from .h to .cpp because of conditional compilation +#if !BT_THREADSAFE + // called in the narrowphase -- not threadsafe! + void* dummy = (void*)(m_box_set.getPrimitiveManager()); + TrimeshPrimitiveManager* dummymanager = static_cast<TrimeshPrimitiveManager*>(dummy); + dummymanager->unlock(); #endif } - #define CALC_EXACT_INERTIA 1 - -void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btGImpactCompoundShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { lockChildShapes(); #ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(0.f, 0.f, 0.f); int i = this->getNumChildShapes(); - btScalar shapemass = mass/btScalar(i); + btScalar shapemass = mass / btScalar(i); - while(i--) + while (i--) { btVector3 temp_inertia; - m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia); - if(childrenHasTransform()) + m_childShapes[i]->calculateLocalInertia(shapemass, temp_inertia); + if (childrenHasTransform()) { - inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]); + inertia = gim_inertia_add_transformed(inertia, temp_inertia, m_childTransforms[i]); } else { - inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity()); + inertia = gim_inertia_add_transformed(inertia, temp_inertia, btTransform::getIdentity()); } - } #else // Calc box inertia - btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; - btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; - btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0]; + btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1]; + btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2]; + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(0.08333333); - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); #endif unlockChildShapes(); } - - -void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass, btVector3& inertia) const { lockChildShapes(); - #ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(0.f, 0.f, 0.f); int i = this->getVertexCount(); - btScalar pointmass = mass/btScalar(i); + btScalar pointmass = mass / btScalar(i); - while(i--) + while (i--) { btVector3 pointintertia; - this->getVertex(i,pointintertia); - pointintertia = gim_get_point_inertia(pointintertia,pointmass); - inertia+=pointintertia; + this->getVertex(i, pointintertia); + pointintertia = gim_get_point_inertia(pointintertia, pointmass); + inertia += pointintertia; } #else // Calc box inertia - btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; - btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; - btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0]; + btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1]; + btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2]; + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(0.08333333); - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); #endif unlockChildShapes(); } -void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btGImpactMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { - #ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); + inertia.setValue(0.f, 0.f, 0.f); int i = this->getMeshPartCount(); - btScalar partmass = mass/btScalar(i); + btScalar partmass = mass / btScalar(i); - while(i--) + while (i--) { btVector3 partinertia; - getMeshPart(i)->calculateLocalInertia(partmass,partinertia); - inertia+=partinertia; + getMeshPart(i)->calculateLocalInertia(partmass, partinertia); + inertia += partinertia; } #else // Calc box inertia - btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; - btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; - btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0]; + btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1]; + btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2]; + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(0.08333333); - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); #endif } @@ -198,7 +189,7 @@ void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayT { } -void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const +void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const { lockChildShapes(); @@ -207,7 +198,7 @@ void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback rayDir.normalize(); m_box_set.rayQuery(rayDir, rayFrom, collided); - if(collided.size()==0) + if (collided.size() == 0) { unlockChildShapes(); return; @@ -216,15 +207,15 @@ void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback int part = (int)getPart(); btPrimitiveTriangle triangle; int i = collided.size(); - while(i--) + while (i--) { - getPrimitiveTriangle(collided[i],triangle); - callback->processTriangle(triangle.m_vertices,part,collided[i]); + getPrimitiveTriangle(collided[i], triangle); + callback->processTriangle(triangle.m_vertices, part, collided[i]); } unlockChildShapes(); } -void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { lockChildShapes(); btAABB box; @@ -232,9 +223,9 @@ void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,co box.m_max = aabbMax; btAlignedObjectArray<int> collided; - m_box_set.boxQuery(box,collided); + m_box_set.boxQuery(box, collided); - if(collided.size()==0) + if (collided.size() == 0) { unlockChildShapes(); return; @@ -243,40 +234,38 @@ void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,co int part = (int)getPart(); btPrimitiveTriangle triangle; int i = collided.size(); - while(i--) + while (i--) { - this->getPrimitiveTriangle(collided[i],triangle); - callback->processTriangle(triangle.m_vertices,part,collided[i]); + this->getPrimitiveTriangle(collided[i], triangle); + callback->processTriangle(triangle.m_vertices, part, collided[i]); } unlockChildShapes(); - } -void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { int i = m_mesh_parts.size(); - while(i--) + while (i--) { - m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax); + m_mesh_parts[i]->processAllTriangles(callback, aabbMin, aabbMax); } } -void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const +void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const { int i = m_mesh_parts.size(); - while(i--) + while (i--) { m_mesh_parts[i]->processAllTrianglesRay(callback, rayFrom, rayTo); } } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*) dataBuffer; + btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*)dataBuffer; - btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer); + btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer); m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer); @@ -288,4 +277,3 @@ const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serial return "btGImpactMeshShapeData"; } - diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h index 9d7e40562c..5b85e87041 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h @@ -21,7 +21,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GIMPACT_SHAPE_H #define GIMPACT_SHAPE_H @@ -37,8 +36,7 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btAlignedObjectArray.h" -#include "btGImpactQuantizedBvh.h" // box tree class - +#include "btGImpactQuantizedBvh.h" // box tree class //! declare Quantized trees, (you can change to float based trees) typedef btGImpactQuantizedBvh btGImpactBoxSet; @@ -50,10 +48,8 @@ enum eGIMPACT_SHAPE_TYPE CONST_GIMPACT_TRIMESH_SHAPE }; - - //! Helper class for tetrahedrons -class btTetrahedronShapeEx:public btBU_Simplex1to4 +class btTetrahedronShapeEx : public btBU_Simplex1to4 { public: btTetrahedronShapeEx() @@ -61,10 +57,9 @@ public: m_numVertices = 4; } - SIMD_FORCE_INLINE void setVertices( - const btVector3 & v0,const btVector3 & v1, - const btVector3 & v2,const btVector3 & v3) + const btVector3& v0, const btVector3& v1, + const btVector3& v2, const btVector3& v3) { m_vertices[0] = v0; m_vertices[1] = v1; @@ -74,45 +69,42 @@ public: } }; - //! Base class for gimpact shapes class btGImpactShapeInterface : public btConcaveShape { protected: - btAABB m_localAABB; - bool m_needs_update; - btVector3 localScaling; - btGImpactBoxSet m_box_set;// optionally boxset + btAABB m_localAABB; + bool m_needs_update; + btVector3 localScaling; + btGImpactBoxSet m_box_set; // optionally boxset //! use this function for perfofm refit in bounding boxes - //! use this function for perfofm refit in bounding boxes - virtual void calcLocalAABB() - { + //! use this function for perfofm refit in bounding boxes + virtual void calcLocalAABB() + { lockChildShapes(); - if(m_box_set.getNodeCount() == 0) - { - m_box_set.buildSet(); - } - else - { - m_box_set.update(); - } - unlockChildShapes(); - - m_localAABB = m_box_set.getGlobalBox(); - } + if (m_box_set.getNodeCount() == 0) + { + m_box_set.buildSet(); + } + else + { + m_box_set.update(); + } + unlockChildShapes(); + m_localAABB = m_box_set.getGlobalBox(); + } public: btGImpactShapeInterface() { - m_shapeType=GIMPACT_SHAPE_PROXYTYPE; + m_shapeType = GIMPACT_SHAPE_PROXYTYPE; m_localAABB.invalidate(); m_needs_update = true; - localScaling.setValue(1.f,1.f,1.f); + localScaling.setValue(1.f, 1.f, 1.f); } - //! performs refit operation /*! Updates the entire Box set of this shape. @@ -120,47 +112,46 @@ public: will does nothing. \post if m_needs_update == true, then it calls calcLocalAABB(); */ - SIMD_FORCE_INLINE void updateBound() - { - if(!m_needs_update) return; - calcLocalAABB(); - m_needs_update = false; - } - - //! If the Bounding box is not updated, then this class attemps to calculate it. - /*! + SIMD_FORCE_INLINE void updateBound() + { + if (!m_needs_update) return; + calcLocalAABB(); + m_needs_update = false; + } + + //! If the Bounding box is not updated, then this class attemps to calculate it. + /*! \post Calls updateBound() for update the box set. */ - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - btAABB transformedbox = m_localAABB; - transformedbox.appy_transform(t); - aabbMin = transformedbox.m_min; - aabbMax = transformedbox.m_max; - } - - //! Tells to this object that is needed to refit the box set - virtual void postUpdate() - { - m_needs_update = true; - } + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + btAABB transformedbox = m_localAABB; + transformedbox.appy_transform(t); + aabbMin = transformedbox.m_min; + aabbMax = transformedbox.m_max; + } + + //! Tells to this object that is needed to refit the box set + virtual void postUpdate() + { + m_needs_update = true; + } //! Obtains the local box, which is the global calculated box of the total of subshapes - SIMD_FORCE_INLINE const btAABB & getLocalBox() + SIMD_FORCE_INLINE const btAABB& getLocalBox() { return m_localAABB; } + virtual int getShapeType() const + { + return GIMPACT_SHAPE_PROXYTYPE; + } - virtual int getShapeType() const - { - return GIMPACT_SHAPE_PROXYTYPE; - } - - /*! + /*! \post You must call updateBound() for update the box set. */ - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { localScaling = scaling; postUpdate(); @@ -171,46 +162,43 @@ public: return localScaling; } - virtual void setMargin(btScalar margin) - { - m_collisionMargin = margin; - int i = getNumChildShapes(); - while(i--) - { + { + m_collisionMargin = margin; + int i = getNumChildShapes(); + while (i--) + { btCollisionShape* child = getChildShape(i); child->setMargin(margin); - } + } m_needs_update = true; - } - + } //! Subshape member functions //!@{ //! Base method for determinig which kind of GIMPACT shape we get - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const = 0 ; + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const = 0; //! gets boxset - SIMD_FORCE_INLINE const btGImpactBoxSet * getBoxSet() const + SIMD_FORCE_INLINE const btGImpactBoxSet* getBoxSet() const { return &m_box_set; } //! Determines if this class has a hierarchy structure for sorting its primitives - SIMD_FORCE_INLINE bool hasBoxSet() const + SIMD_FORCE_INLINE bool hasBoxSet() const { - if(m_box_set.getNodeCount() == 0) return false; + if (m_box_set.getNodeCount() == 0) return false; return true; } //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const = 0; - + virtual const btPrimitiveManagerBase* getPrimitiveManager() const = 0; //! Gets the number of children - virtual int getNumChildShapes() const = 0; + virtual int getNumChildShapes() const = 0; //! if true, then its children must get transforms. virtual bool childrenHasTransform() const = 0; @@ -221,11 +209,9 @@ public: //! Determines if this shape has tetrahedrons virtual bool needsRetrieveTetrahedrons() const = 0; - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const = 0; - - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const = 0; - + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const = 0; + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const = 0; //! call when reading child shapes virtual void lockChildShapes() const @@ -237,94 +223,91 @@ public: } //! if this trimesh - SIMD_FORCE_INLINE void getPrimitiveTriangle(int index,btPrimitiveTriangle & triangle) const + SIMD_FORCE_INLINE void getPrimitiveTriangle(int index, btPrimitiveTriangle& triangle) const { - getPrimitiveManager()->get_primitive_triangle(index,triangle); + getPrimitiveManager()->get_primitive_triangle(index, triangle); } - //! Retrieves the bound from a child - /*! + /*! */ - virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - btAABB child_aabb; - getPrimitiveManager()->get_primitive_box(child_index,child_aabb); - child_aabb.appy_transform(t); - aabbMin = child_aabb.m_min; - aabbMax = child_aabb.m_max; - } + virtual void getChildAabb(int child_index, const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + btAABB child_aabb; + getPrimitiveManager()->get_primitive_box(child_index, child_aabb); + child_aabb.appy_transform(t); + aabbMin = child_aabb.m_min; + aabbMax = child_aabb.m_max; + } //! Gets the children virtual btCollisionShape* getChildShape(int index) = 0; - //! Gets the child virtual const btCollisionShape* getChildShape(int index) const = 0; //! Gets the children transform - virtual btTransform getChildTransform(int index) const = 0; + virtual btTransform getChildTransform(int index) const = 0; //! Sets the children transform /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) = 0; + virtual void setChildTransform(int index, const btTransform& transform) = 0; //!@} - //! virtual method for ray collision - virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const { - (void) rayFrom; (void) rayTo; (void) resultCallback; + (void)rayFrom; + (void)rayTo; + (void)resultCallback; } //! Function for retrieve triangles. /*! It gives the triangles in local space */ - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const + virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - (void) callback; (void) aabbMin; (void) aabbMax; + (void)callback; + (void)aabbMin; + (void)aabbMax; } //! Function for retrieve triangles. /*! It gives the triangles in local space */ - virtual void processAllTrianglesRay(btTriangleCallback* /*callback*/,const btVector3& /*rayFrom*/, const btVector3& /*rayTo*/) const + virtual void processAllTrianglesRay(btTriangleCallback* /*callback*/, const btVector3& /*rayFrom*/, const btVector3& /*rayTo*/) const { - } //!@} - }; - //! btGImpactCompoundShape allows to handle multiple btCollisionShape objects at once /*! This class only can manage Convex subshapes */ -class btGImpactCompoundShape : public btGImpactShapeInterface +class btGImpactCompoundShape : public btGImpactShapeInterface { public: //! compound primitive manager - class CompoundPrimitiveManager:public btPrimitiveManagerBase + class CompoundPrimitiveManager : public btPrimitiveManagerBase { public: virtual ~CompoundPrimitiveManager() {} - btGImpactCompoundShape * m_compoundShape; - + btGImpactCompoundShape* m_compoundShape; CompoundPrimitiveManager(const CompoundPrimitiveManager& compound) - : btPrimitiveManagerBase() + : btPrimitiveManagerBase() { m_compoundShape = compound.m_compoundShape; } - CompoundPrimitiveManager(btGImpactCompoundShape * compoundShape) + CompoundPrimitiveManager(btGImpactCompoundShape* compoundShape) { m_compoundShape = compoundShape; } @@ -341,13 +324,13 @@ public: virtual int get_primitive_count() const { - return (int )m_compoundShape->getNumChildShapes(); + return (int)m_compoundShape->getNumChildShapes(); } - virtual void get_primitive_box(int prim_index ,btAABB & primbox) const + virtual void get_primitive_box(int prim_index, btAABB& primbox) const { btTransform prim_trans; - if(m_compoundShape->childrenHasTransform()) + if (m_compoundShape->childrenHasTransform()) { prim_trans = m_compoundShape->getChildTransform(prim_index); } @@ -356,30 +339,26 @@ public: prim_trans.setIdentity(); } const btCollisionShape* shape = m_compoundShape->getChildShape(prim_index); - shape->getAabb(prim_trans,primbox.m_min,primbox.m_max); + shape->getAabb(prim_trans, primbox.m_min, primbox.m_max); } - virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const + virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const { btAssert(0); - (void) prim_index; (void) triangle; + (void)prim_index; + (void)triangle; } - }; - - protected: CompoundPrimitiveManager m_primitive_manager; - btAlignedObjectArray<btTransform> m_childTransforms; - btAlignedObjectArray<btCollisionShape*> m_childShapes; - + btAlignedObjectArray<btTransform> m_childTransforms; + btAlignedObjectArray<btCollisionShape*> m_childShapes; public: - btGImpactCompoundShape(bool children_has_transform = true) { - (void) children_has_transform; + (void)children_has_transform; m_primitive_manager.m_compoundShape = this; m_box_set.setPrimitiveManager(&m_primitive_manager); } @@ -388,36 +367,33 @@ public: { } - //! if true, then its children must get transforms. virtual bool childrenHasTransform() const { - if(m_childTransforms.size()==0) return false; + if (m_childTransforms.size() == 0) return false; return true; } - //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const + virtual const btPrimitiveManagerBase* getPrimitiveManager() const { return &m_primitive_manager; } //! Obtains the compopund primitive manager - SIMD_FORCE_INLINE CompoundPrimitiveManager * getCompoundPrimitiveManager() + SIMD_FORCE_INLINE CompoundPrimitiveManager* getCompoundPrimitiveManager() { return &m_primitive_manager; } //! Gets the number of children - virtual int getNumChildShapes() const + virtual int getNumChildShapes() const { return m_childShapes.size(); } - //! Use this method for adding children. Only Convex shapes are allowed. - void addChildShape(const btTransform& localTransform,btCollisionShape* shape) + void addChildShape(const btTransform& localTransform, btCollisionShape* shape) { btAssert(shape->isConvex()); m_childTransforms.push_back(localTransform); @@ -444,24 +420,22 @@ public: } //! Retrieves the bound from a child - /*! + /*! */ - virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - - if(childrenHasTransform()) - { - m_childShapes[child_index]->getAabb(t*m_childTransforms[child_index],aabbMin,aabbMax); - } - else - { - m_childShapes[child_index]->getAabb(t,aabbMin,aabbMax); - } - } - + virtual void getChildAabb(int child_index, const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + if (childrenHasTransform()) + { + m_childShapes[child_index]->getAabb(t * m_childTransforms[child_index], aabbMin, aabbMax); + } + else + { + m_childShapes[child_index]->getAabb(t, aabbMin, aabbMax); + } + } //! Gets the children transform - virtual btTransform getChildTransform(int index) const + virtual btTransform getChildTransform(int index) const { btAssert(m_childTransforms.size() == m_childShapes.size()); return m_childTransforms[index]; @@ -471,7 +445,7 @@ public: /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) + virtual void setChildTransform(int index, const btTransform& transform) { btAssert(m_childTransforms.size() == m_childShapes.size()); m_childTransforms[index] = transform; @@ -490,24 +464,24 @@ public: return false; } - - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const { - (void) prim_index; (void) triangle; + (void)prim_index; + (void)triangle; btAssert(0); } - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const { - (void) prim_index; (void) tetrahedron; + (void)prim_index; + (void)tetrahedron; btAssert(0); } - //! Calculates the exact inertia tensor for this shape - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const; - virtual const char* getName()const + virtual const char* getName() const { return "GImpactCompound"; } @@ -516,11 +490,8 @@ public: { return CONST_GIMPACT_COMPOUND_SHAPE; } - }; - - //! This class manages a sub part of a mesh supplied by the btStridingMeshInterface interface. /*! - Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShapePart, then you must call updateBound() after creating the mesh @@ -535,21 +506,21 @@ public: /*! Manages the info from btStridingMeshInterface object and controls the Lock/Unlock mechanism */ - class TrimeshPrimitiveManager:public btPrimitiveManagerBase + class TrimeshPrimitiveManager : public btPrimitiveManagerBase { public: btScalar m_margin; - btStridingMeshInterface * m_meshInterface; + btStridingMeshInterface* m_meshInterface; btVector3 m_scale; int m_part; int m_lock_count; - const unsigned char *vertexbase; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; - int numfaces; + int numfaces; PHY_ScalarType indicestype; TrimeshPrimitiveManager() @@ -557,7 +528,7 @@ public: m_meshInterface = NULL; m_part = 0; m_margin = 0.01f; - m_scale = btVector3(1.f,1.f,1.f); + m_scale = btVector3(1.f, 1.f, 1.f); m_lock_count = 0; vertexbase = 0; numverts = 0; @@ -567,8 +538,8 @@ public: numfaces = 0; } - TrimeshPrimitiveManager(const TrimeshPrimitiveManager & manager) - : btPrimitiveManagerBase() + TrimeshPrimitiveManager(const TrimeshPrimitiveManager& manager) + : btPrimitiveManagerBase() { m_meshInterface = manager.m_meshInterface; m_part = manager.m_part; @@ -581,11 +552,10 @@ public: indexbase = 0; indexstride = 0; numfaces = 0; - } TrimeshPrimitiveManager( - btStridingMeshInterface * meshInterface, int part) + btStridingMeshInterface* meshInterface, int part) { m_meshInterface = meshInterface; m_part = part; @@ -598,29 +568,28 @@ public: indexbase = 0; indexstride = 0; numfaces = 0; - } virtual ~TrimeshPrimitiveManager() {} void lock() { - if(m_lock_count>0) + if (m_lock_count > 0) { m_lock_count++; return; } m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase,numverts, - type, stride,&indexbase, indexstride, numfaces,indicestype,m_part); + &vertexbase, numverts, + type, stride, &indexbase, indexstride, numfaces, indicestype, m_part); m_lock_count = 1; } void unlock() { - if(m_lock_count == 0) return; - if(m_lock_count>1) + if (m_lock_count == 0) return; + if (m_lock_count > 1) { --m_lock_count; return; @@ -637,93 +606,91 @@ public: virtual int get_primitive_count() const { - return (int )numfaces; + return (int)numfaces; } SIMD_FORCE_INLINE int get_vertex_count() const { - return (int )numverts; + return (int)numverts; } - SIMD_FORCE_INLINE void get_indices(int face_index,unsigned int &i0,unsigned int &i1,unsigned int &i2) const + SIMD_FORCE_INLINE void get_indices(int face_index, unsigned int& i0, unsigned int& i1, unsigned int& i2) const { - if(indicestype == PHY_SHORT) + if (indicestype == PHY_SHORT) { - unsigned short* s_indices = (unsigned short *)(indexbase + face_index * indexstride); + unsigned short* s_indices = (unsigned short*)(indexbase + face_index * indexstride); i0 = s_indices[0]; i1 = s_indices[1]; i2 = s_indices[2]; } else { - unsigned int * i_indices = (unsigned int *)(indexbase + face_index*indexstride); + unsigned int* i_indices = (unsigned int*)(indexbase + face_index * indexstride); i0 = i_indices[0]; i1 = i_indices[1]; i2 = i_indices[2]; } } - SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3 & vertex) const + SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3& vertex) const { - if(type == PHY_DOUBLE) + if (type == PHY_DOUBLE) { - double * dvertices = (double *)(vertexbase + vertex_index*stride); - vertex[0] = btScalar(dvertices[0]*m_scale[0]); - vertex[1] = btScalar(dvertices[1]*m_scale[1]); - vertex[2] = btScalar(dvertices[2]*m_scale[2]); + double* dvertices = (double*)(vertexbase + vertex_index * stride); + vertex[0] = btScalar(dvertices[0] * m_scale[0]); + vertex[1] = btScalar(dvertices[1] * m_scale[1]); + vertex[2] = btScalar(dvertices[2] * m_scale[2]); } else { - float * svertices = (float *)(vertexbase + vertex_index*stride); - vertex[0] = svertices[0]*m_scale[0]; - vertex[1] = svertices[1]*m_scale[1]; - vertex[2] = svertices[2]*m_scale[2]; + float* svertices = (float*)(vertexbase + vertex_index * stride); + vertex[0] = svertices[0] * m_scale[0]; + vertex[1] = svertices[1] * m_scale[1]; + vertex[2] = svertices[2] * m_scale[2]; } } - virtual void get_primitive_box(int prim_index ,btAABB & primbox) const + virtual void get_primitive_box(int prim_index, btAABB& primbox) const { - btPrimitiveTriangle triangle; - get_primitive_triangle(prim_index,triangle); + btPrimitiveTriangle triangle; + get_primitive_triangle(prim_index, triangle); primbox.calc_from_triangle_margin( triangle.m_vertices[0], - triangle.m_vertices[1],triangle.m_vertices[2],triangle.m_margin); + triangle.m_vertices[1], triangle.m_vertices[2], triangle.m_margin); } - virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const + virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const { unsigned int indices[3]; - get_indices(prim_index,indices[0],indices[1],indices[2]); - get_vertex(indices[0],triangle.m_vertices[0]); - get_vertex(indices[1],triangle.m_vertices[1]); - get_vertex(indices[2],triangle.m_vertices[2]); + get_indices(prim_index, indices[0], indices[1], indices[2]); + get_vertex(indices[0], triangle.m_vertices[0]); + get_vertex(indices[1], triangle.m_vertices[1]); + get_vertex(indices[2], triangle.m_vertices[2]); triangle.m_margin = m_margin; } - SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index,btTriangleShapeEx & triangle) const + SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index, btTriangleShapeEx& triangle) const { unsigned int indices[3]; - get_indices(prim_index,indices[0],indices[1],indices[2]); - get_vertex(indices[0],triangle.m_vertices1[0]); - get_vertex(indices[1],triangle.m_vertices1[1]); - get_vertex(indices[2],triangle.m_vertices1[2]); + get_indices(prim_index, indices[0], indices[1], indices[2]); + get_vertex(indices[0], triangle.m_vertices1[0]); + get_vertex(indices[1], triangle.m_vertices1[1]); + get_vertex(indices[2], triangle.m_vertices1[2]); triangle.setMargin(m_margin); } - }; - protected: TrimeshPrimitiveManager m_primitive_manager; -public: +public: btGImpactMeshShapePart() { m_box_set.setPrimitiveManager(&m_primitive_manager); } - btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part ); - virtual ~btGImpactMeshShapePart(); + btGImpactMeshShapePart(btStridingMeshInterface* meshInterface, int part); + virtual ~btGImpactMeshShapePart(); //! if true, then its children must get transforms. virtual bool childrenHasTransform() const @@ -731,40 +698,36 @@ public: return false; } - //! call when reading child shapes - virtual void lockChildShapes() const; - virtual void unlockChildShapes() const; + virtual void lockChildShapes() const; + virtual void unlockChildShapes() const; //! Gets the number of children - virtual int getNumChildShapes() const + virtual int getNumChildShapes() const { return m_primitive_manager.get_primitive_count(); } - //! Gets the children virtual btCollisionShape* getChildShape(int index) { - (void) index; + (void)index; btAssert(0); return NULL; } - - //! Gets the child virtual const btCollisionShape* getChildShape(int index) const { - (void) index; + (void)index; btAssert(0); return NULL; } //! Gets the children transform - virtual btTransform getChildTransform(int index) const + virtual btTransform getChildTransform(int index) const { - (void) index; + (void)index; btAssert(0); return btTransform(); } @@ -773,35 +736,27 @@ public: /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) + virtual void setChildTransform(int index, const btTransform& transform) { - (void) index; - (void) transform; + (void)index; + (void)transform; btAssert(0); } - //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const + virtual const btPrimitiveManagerBase* getPrimitiveManager() const { return &m_primitive_manager; } - SIMD_FORCE_INLINE TrimeshPrimitiveManager * getTrimeshPrimitiveManager() + SIMD_FORCE_INLINE TrimeshPrimitiveManager* getTrimeshPrimitiveManager() { return &m_primitive_manager; } + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const; - - - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - - - - virtual const char* getName()const + virtual const char* getName() const { return "GImpactMeshShapePart"; } @@ -823,61 +778,58 @@ public: return false; } - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const { - m_primitive_manager.get_bullet_triangle(prim_index,triangle); + m_primitive_manager.get_bullet_triangle(prim_index, triangle); } - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const { - (void) prim_index; - (void) tetrahedron; + (void)prim_index; + (void)tetrahedron; btAssert(0); } - - SIMD_FORCE_INLINE int getVertexCount() const { return m_primitive_manager.get_vertex_count(); } - SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3 & vertex) const + SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3& vertex) const { - m_primitive_manager.get_vertex(vertex_index,vertex); + m_primitive_manager.get_vertex(vertex_index, vertex); } SIMD_FORCE_INLINE void setMargin(btScalar margin) - { - m_primitive_manager.m_margin = margin; - postUpdate(); - } - - SIMD_FORCE_INLINE btScalar getMargin() const - { - return m_primitive_manager.m_margin; - } - - virtual void setLocalScaling(const btVector3& scaling) - { - m_primitive_manager.m_scale = scaling; - postUpdate(); - } - - virtual const btVector3& getLocalScaling() const - { - return m_primitive_manager.m_scale; - } - - SIMD_FORCE_INLINE int getPart() const - { - return (int)m_primitive_manager.m_part; - } - - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - virtual void processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom,const btVector3& rayTo) const; -}; + { + m_primitive_manager.m_margin = margin; + postUpdate(); + } + + SIMD_FORCE_INLINE btScalar getMargin() const + { + return m_primitive_manager.m_margin; + } + + virtual void setLocalScaling(const btVector3& scaling) + { + m_primitive_manager.m_scale = scaling; + postUpdate(); + } + + virtual const btVector3& getLocalScaling() const + { + return m_primitive_manager.m_scale; + } + SIMD_FORCE_INLINE int getPart() const + { + return (int)m_primitive_manager.m_part; + } + + virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const; + virtual void processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const; +}; //! This class manages a mesh supplied by the btStridingMeshInterface interface. /*! @@ -893,29 +845,29 @@ class btGImpactMeshShape : public btGImpactShapeInterface protected: btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts; - void buildMeshParts(btStridingMeshInterface * meshInterface) + void buildMeshParts(btStridingMeshInterface* meshInterface) { - for (int i=0;i<meshInterface->getNumSubParts() ;++i ) + for (int i = 0; i < meshInterface->getNumSubParts(); ++i) { - btGImpactMeshShapePart * newpart = new btGImpactMeshShapePart(meshInterface,i); + btGImpactMeshShapePart* newpart = new btGImpactMeshShapePart(meshInterface, i); m_mesh_parts.push_back(newpart); } } //! use this function for perfofm refit in bounding boxes - virtual void calcLocalAABB() - { - m_localAABB.invalidate(); - int i = m_mesh_parts.size(); - while(i--) - { - m_mesh_parts[i]->updateBound(); - m_localAABB.merge(m_mesh_parts[i]->getLocalBox()); - } - } + virtual void calcLocalAABB() + { + m_localAABB.invalidate(); + int i = m_mesh_parts.size(); + while (i--) + { + m_mesh_parts[i]->updateBound(); + m_localAABB.merge(m_mesh_parts[i]->getLocalBox()); + } + } public: - btGImpactMeshShape(btStridingMeshInterface * meshInterface) + btGImpactMeshShape(btStridingMeshInterface* meshInterface) { m_meshInterface = meshInterface; buildMeshParts(meshInterface); @@ -924,15 +876,14 @@ public: virtual ~btGImpactMeshShape() { int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; delete part; - } + } m_mesh_parts.clear(); } - btStridingMeshInterface* getMeshInterface() { return m_meshInterface; @@ -948,79 +899,73 @@ public: return m_mesh_parts.size(); } - btGImpactMeshShapePart * getMeshPart(int index) + btGImpactMeshShapePart* getMeshPart(int index) { return m_mesh_parts[index]; } - - - const btGImpactMeshShapePart * getMeshPart(int index) const + const btGImpactMeshShapePart* getMeshPart(int index) const { return m_mesh_parts[index]; } - - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { localScaling = scaling; int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; part->setLocalScaling(scaling); - } + } m_needs_update = true; } virtual void setMargin(btScalar margin) - { - m_collisionMargin = margin; + { + m_collisionMargin = margin; int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; part->setMargin(margin); - } + } m_needs_update = true; - } + } //! Tells to this object that is needed to refit all the meshes - virtual void postUpdate() - { + virtual void postUpdate() + { int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; + while (i--) + { + btGImpactMeshShapePart* part = m_mesh_parts[i]; part->postUpdate(); - } - - m_needs_update = true; - } + } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + m_needs_update = true; + } + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const; //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const + virtual const btPrimitiveManagerBase* getPrimitiveManager() const { btAssert(0); return NULL; } - //! Gets the number of children - virtual int getNumChildShapes() const + virtual int getNumChildShapes() const { btAssert(0); return 0; } - //! if true, then its children must get transforms. virtual bool childrenHasTransform() const { @@ -1042,15 +987,17 @@ public: return false; } - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + virtual void getBulletTriangle(int prim_index, btTriangleShapeEx& triangle) const { - (void) prim_index; (void) triangle; + (void)prim_index; + (void)triangle; btAssert(0); } - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + virtual void getBulletTetrahedron(int prim_index, btTetrahedronShapeEx& tetrahedron) const { - (void) prim_index; (void) tetrahedron; + (void)prim_index; + (void)tetrahedron; btAssert(0); } @@ -1065,39 +1012,38 @@ public: btAssert(0); } - - - //! Retrieves the bound from a child - /*! + /*! */ - virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - (void) child_index; (void) t; (void) aabbMin; (void) aabbMax; - btAssert(0); - } + virtual void getChildAabb(int child_index, const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + (void)child_index; + (void)t; + (void)aabbMin; + (void)aabbMax; + btAssert(0); + } //! Gets the children virtual btCollisionShape* getChildShape(int index) { - (void) index; + (void)index; btAssert(0); return NULL; } - //! Gets the child virtual const btCollisionShape* getChildShape(int index) const { - (void) index; + (void)index; btAssert(0); return NULL; } //! Gets the children transform - virtual btTransform getChildTransform(int index) const + virtual btTransform getChildTransform(int index) const { - (void) index; + (void)index; btAssert(0); return btTransform(); } @@ -1106,59 +1052,56 @@ public: /*! \post You must call updateBound() for update the box set. */ - virtual void setChildTransform(int index, const btTransform & transform) + virtual void setChildTransform(int index, const btTransform& transform) { - (void) index; (void) transform; + (void)index; + (void)transform; btAssert(0); } - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const { return CONST_GIMPACT_TRIMESH_SHAPE; } - - virtual const char* getName()const + virtual const char* getName() const { return "GImpactMesh"; } - virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const; + virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const; //! Function for retrieve triangles. /*! It gives the triangles in local space */ - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void processAllTrianglesRay (btTriangleCallback* callback,const btVector3& rayFrom,const btVector3& rayTo) const; + virtual void processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const; - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btGImpactMeshShapeData +struct btGImpactMeshShapeData { - btCollisionShapeData m_collisionShapeData; + btCollisionShapeData m_collisionShapeData; btStridingMeshInterfaceData m_meshInterface; - btVector3FloatData m_localScaling; + btVector3FloatData m_localScaling; - float m_collisionMargin; + float m_collisionMargin; - int m_gimpactSubType; + int m_gimpactSubType; }; -SIMD_FORCE_INLINE int btGImpactMeshShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btGImpactMeshShape::calculateSerializeBufferSize() const { return sizeof(btGImpactMeshShapeData); } - -#endif //GIMPACT_MESH_SHAPE_H +#endif //GIMPACT_MESH_SHAPE_H diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.cpp index 5d07d1adb9..bfdb3db5d0 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.cpp @@ -20,48 +20,45 @@ subject to the following restrictions: #include "btGenericPoolAllocator.h" - - /// *************** btGenericMemoryPool ******************/////////// size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements) { size_t ptr = BT_UINT_MAX; - if(m_free_nodes_count == 0) return BT_UINT_MAX; + if (m_free_nodes_count == 0) return BT_UINT_MAX; // find an avaliable free node with the correct size size_t revindex = m_free_nodes_count; - while(revindex-- && ptr == BT_UINT_MAX) + while (revindex-- && ptr == BT_UINT_MAX) { - if(m_allocated_sizes[m_free_nodes[revindex]]>=num_elements) + if (m_allocated_sizes[m_free_nodes[revindex]] >= num_elements) { ptr = revindex; } } - if(ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found - + if (ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found revindex = ptr; ptr = m_free_nodes[revindex]; // post: ptr contains the node index, and revindex the index in m_free_nodes - size_t finalsize = m_allocated_sizes[ptr]; + size_t finalsize = m_allocated_sizes[ptr]; finalsize -= num_elements; m_allocated_sizes[ptr] = num_elements; // post: finalsize>=0, m_allocated_sizes[ptr] has the requested size - if(finalsize>0) // preserve free node, there are some free memory + if (finalsize > 0) // preserve free node, there are some free memory { m_free_nodes[revindex] = ptr + num_elements; m_allocated_sizes[ptr + num_elements] = finalsize; } - else // delete free node + else // delete free node { // swap with end - m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count-1]; + m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count - 1]; m_free_nodes_count--; } @@ -70,17 +67,16 @@ size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements) size_t btGenericMemoryPool::allocate_from_pool(size_t num_elements) { - if(m_allocated_count+num_elements>m_max_element_count) return BT_UINT_MAX; + if (m_allocated_count + num_elements > m_max_element_count) return BT_UINT_MAX; size_t ptr = m_allocated_count; m_allocated_sizes[m_allocated_count] = num_elements; - m_allocated_count+=num_elements; + m_allocated_count += num_elements; return ptr; } - void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count) { m_allocated_count = 0; @@ -89,14 +85,11 @@ void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count) m_element_size = element_size; m_max_element_count = element_count; + m_pool = (unsigned char *)btAlignedAlloc(m_element_size * m_max_element_count, 16); + m_free_nodes = (size_t *)btAlignedAlloc(sizeof(size_t) * m_max_element_count, 16); + m_allocated_sizes = (size_t *)btAlignedAlloc(sizeof(size_t) * m_max_element_count, 16); - - - m_pool = (unsigned char *) btAlignedAlloc(m_element_size*m_max_element_count,16); - m_free_nodes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16); - m_allocated_sizes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16); - - for (size_t i = 0;i< m_max_element_count;i++ ) + for (size_t i = 0; i < m_max_element_count; i++) { m_allocated_sizes[i] = 0; } @@ -111,150 +104,141 @@ void btGenericMemoryPool::end_pool() m_free_nodes_count = 0; } - //! Allocates memory in pool /*! \param size_bytes size in bytes of the buffer */ -void * btGenericMemoryPool::allocate(size_t size_bytes) +void *btGenericMemoryPool::allocate(size_t size_bytes) { - - size_t module = size_bytes%m_element_size; - size_t element_count = size_bytes/m_element_size; - if(module>0) element_count++; + size_t module = size_bytes % m_element_size; + size_t element_count = size_bytes / m_element_size; + if (module > 0) element_count++; size_t alloc_pos = allocate_from_free_nodes(element_count); // a free node is found - if(alloc_pos != BT_UINT_MAX) + if (alloc_pos != BT_UINT_MAX) { return get_element_data(alloc_pos); } // allocate directly on pool alloc_pos = allocate_from_pool(element_count); - if(alloc_pos == BT_UINT_MAX) return NULL; // not space + if (alloc_pos == BT_UINT_MAX) return NULL; // not space return get_element_data(alloc_pos); } -bool btGenericMemoryPool::freeMemory(void * pointer) +bool btGenericMemoryPool::freeMemory(void *pointer) { - unsigned char * pointer_pos = (unsigned char *)pointer; - unsigned char * pool_pos = (unsigned char *)m_pool; + unsigned char *pointer_pos = (unsigned char *)pointer; + unsigned char *pool_pos = (unsigned char *)m_pool; // calc offset - if(pointer_pos<pool_pos) return false;//other pool + if (pointer_pos < pool_pos) return false; //other pool size_t offset = size_t(pointer_pos - pool_pos); - if(offset>=get_pool_capacity()) return false;// far away + if (offset >= get_pool_capacity()) return false; // far away // find free position - m_free_nodes[m_free_nodes_count] = offset/m_element_size; + m_free_nodes[m_free_nodes_count] = offset / m_element_size; m_free_nodes_count++; return true; } - /// *******************! btGenericPoolAllocator *******************!/// - btGenericPoolAllocator::~btGenericPoolAllocator() { // destroy pools size_t i; - for (i=0;i<m_pool_count;i++) + for (i = 0; i < m_pool_count; i++) { m_pools[i]->end_pool(); btAlignedFree(m_pools[i]); } } - // creates a pool -btGenericMemoryPool * btGenericPoolAllocator::push_new_pool() +btGenericMemoryPool *btGenericPoolAllocator::push_new_pool() { - if(m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL; + if (m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL; - btGenericMemoryPool * newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool),16); + btGenericMemoryPool *newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool), 16); m_pools[m_pool_count] = newptr; - m_pools[m_pool_count]->init_pool(m_pool_element_size,m_pool_element_count); + m_pools[m_pool_count]->init_pool(m_pool_element_size, m_pool_element_count); m_pool_count++; return newptr; } -void * btGenericPoolAllocator::failback_alloc(size_t size_bytes) +void *btGenericPoolAllocator::failback_alloc(size_t size_bytes) { + btGenericMemoryPool *pool = NULL; - btGenericMemoryPool * pool = NULL; - - - if(size_bytes<=get_pool_capacity()) + if (size_bytes <= get_pool_capacity()) { - pool = push_new_pool(); + pool = push_new_pool(); } - if(pool==NULL) // failback + if (pool == NULL) // failback { - return btAlignedAlloc(size_bytes,16); + return btAlignedAlloc(size_bytes, 16); } return pool->allocate(size_bytes); } -bool btGenericPoolAllocator::failback_free(void * pointer) +bool btGenericPoolAllocator::failback_free(void *pointer) { btAlignedFree(pointer); return true; } - //! Allocates memory in pool /*! \param size_bytes size in bytes of the buffer */ -void * btGenericPoolAllocator::allocate(size_t size_bytes) +void *btGenericPoolAllocator::allocate(size_t size_bytes) { - void * ptr = NULL; + void *ptr = NULL; size_t i = 0; - while(i<m_pool_count && ptr == NULL) + while (i < m_pool_count && ptr == NULL) { ptr = m_pools[i]->allocate(size_bytes); ++i; } - if(ptr) return ptr; + if (ptr) return ptr; return failback_alloc(size_bytes); } -bool btGenericPoolAllocator::freeMemory(void * pointer) +bool btGenericPoolAllocator::freeMemory(void *pointer) { bool result = false; size_t i = 0; - while(i<m_pool_count && result == false) + while (i < m_pool_count && result == false) { result = m_pools[i]->freeMemory(pointer); ++i; } - if(result) return true; + if (result) return true; return failback_free(pointer); } /// ************** STANDARD ALLOCATOR ***************************/// - #define BT_DEFAULT_POOL_SIZE 32768 #define BT_DEFAULT_POOL_ELEMENT_SIZE 8 // main allocator -class GIM_STANDARD_ALLOCATOR: public btGenericPoolAllocator +class GIM_STANDARD_ALLOCATOR : public btGenericPoolAllocator { public: - GIM_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE) + GIM_STANDARD_ALLOCATOR() : btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE, BT_DEFAULT_POOL_SIZE) { } }; @@ -262,19 +246,18 @@ public: // global allocator GIM_STANDARD_ALLOCATOR g_main_allocator; - -void * btPoolAlloc(size_t size) +void *btPoolAlloc(size_t size) { return g_main_allocator.allocate(size); } -void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize) +void *btPoolRealloc(void *ptr, size_t oldsize, size_t newsize) { - void * newptr = btPoolAlloc(newsize); - size_t copysize = oldsize<newsize?oldsize:newsize; - memcpy(newptr,ptr,copysize); - btPoolFree(ptr); - return newptr; + void *newptr = btPoolAlloc(newsize); + size_t copysize = oldsize < newsize ? oldsize : newsize; + memcpy(newptr, ptr, copysize); + btPoolFree(ptr); + return newptr; } void btPoolFree(void *ptr) diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.h b/thirdparty/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.h index b46d851634..a535088e48 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGenericPoolAllocator.h @@ -29,16 +29,16 @@ subject to the following restrictions: #define BT_UINT_MAX UINT_MAX #define BT_DEFAULT_MAX_POOLS 16 - //! Generic Pool class class btGenericMemoryPool { public: - unsigned char * m_pool; //[m_element_size*m_max_element_count]; - size_t * m_free_nodes; //[m_max_element_count];//! free nodes - size_t * m_allocated_sizes;//[m_max_element_count];//! Number of elements allocated per node + unsigned char *m_pool; //[m_element_size*m_max_element_count]; + size_t *m_free_nodes; //[m_max_element_count];//! free nodes + size_t *m_allocated_sizes; //[m_max_element_count];//! Number of elements allocated per node size_t m_allocated_count; size_t m_free_nodes_count; + protected: size_t m_element_size; size_t m_max_element_count; @@ -47,12 +47,10 @@ protected: size_t allocate_from_pool(size_t num_elements); public: - void init_pool(size_t element_size, size_t element_count); void end_pool(); - btGenericMemoryPool(size_t element_size, size_t element_count) { init_pool(element_size, element_count); @@ -63,10 +61,9 @@ public: end_pool(); } - inline size_t get_pool_capacity() { - return m_element_size*m_max_element_count; + return m_element_size * m_max_element_count; } inline size_t gem_element_size() @@ -89,23 +86,20 @@ public: return m_free_nodes_count; } - inline void * get_element_data(size_t element_index) + inline void *get_element_data(size_t element_index) { - return &m_pool[element_index*m_element_size]; + return &m_pool[element_index * m_element_size]; } //! Allocates memory in pool /*! \param size_bytes size in bytes of the buffer */ - void * allocate(size_t size_bytes); + void *allocate(size_t size_bytes); - bool freeMemory(void * pointer); + bool freeMemory(void *pointer); }; - - - //! Generic Allocator with pools /*! General purpose Allocator which can create Memory Pools dynamiacally as needed. @@ -115,26 +109,25 @@ class btGenericPoolAllocator protected: size_t m_pool_element_size; size_t m_pool_element_count; + public: - btGenericMemoryPool * m_pools[BT_DEFAULT_MAX_POOLS]; + btGenericMemoryPool *m_pools[BT_DEFAULT_MAX_POOLS]; size_t m_pool_count; - inline size_t get_pool_capacity() { - return m_pool_element_size*m_pool_element_count; + return m_pool_element_size * m_pool_element_count; } - protected: // creates a pool - btGenericMemoryPool * push_new_pool(); + btGenericMemoryPool *push_new_pool(); - void * failback_alloc(size_t size_bytes); + void *failback_alloc(size_t size_bytes); - bool failback_free(void * pointer); -public: + bool failback_free(void *pointer); +public: btGenericPoolAllocator(size_t pool_element_size, size_t pool_element_count) { m_pool_count = 0; @@ -148,16 +141,13 @@ public: /*! \param size_bytes size in bytes of the buffer */ - void * allocate(size_t size_bytes); + void *allocate(size_t size_bytes); - bool freeMemory(void * pointer); + bool freeMemory(void *pointer); }; - - -void * btPoolAlloc(size_t size); -void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize); +void *btPoolAlloc(size_t size); +void *btPoolRealloc(void *ptr, size_t oldsize, size_t newsize); void btPoolFree(void *ptr); - #endif diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h b/thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h index 60f06510ad..6a1ee6dcf9 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGeometryOperations.h @@ -27,52 +27,44 @@ subject to the following restrictions: #include "btBoxCollision.h" - - - - #define PLANEDIREPSILON 0.0000001f #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 -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(); - 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 **********************************/// /*! Finds the closest point(cp) to (v) on a segment (e1,e2) */ SIMD_FORCE_INLINE void bt_closest_point_on_segment( - btVector3 & cp, const btVector3 & v, - const btVector3 &e1,const btVector3 &e2) + btVector3 &cp, const btVector3 &v, + const btVector3 &e1, const btVector3 &e2) { - btVector3 n = e2-e1; - cp = v - e1; - btScalar _scalar = cp.dot(n)/n.dot(n); - if(_scalar <0.0f) + btVector3 n = e2 - e1; + cp = v - e1; + btScalar _scalar = cp.dot(n) / n.dot(n); + if (_scalar < 0.0f) { - cp = e1; + cp = e1; } - else if(_scalar >1.0f) + else if (_scalar > 1.0f) { - cp = e2; + cp = e2; } else { - cp = _scalar*n + e1; + cp = _scalar * n + e1; } } - //! line plane collision /*! *\return @@ -82,131 +74,125 @@ SIMD_FORCE_INLINE void bt_closest_point_on_segment( */ SIMD_FORCE_INLINE int bt_line_plane_collision( - const btVector4 & plane, - const btVector3 & vDir, - const btVector3 & vPoint, - btVector3 & pout, + const btVector4 &plane, + const btVector3 &vDir, + const btVector3 &vPoint, + btVector3 &pout, btScalar &tparam, btScalar tmin, btScalar tmax) { - btScalar _dotdir = vDir.dot(plane); - if(btFabs(_dotdir)<PLANEDIREPSILON) + if (btFabs(_dotdir) < PLANEDIREPSILON) { tparam = tmax; - return 0; + return 0; } - btScalar _dis = bt_distance_point_plane(plane,vPoint); - char returnvalue = _dis<0.0f? 2:1; - tparam = -_dis/_dotdir; + btScalar _dis = bt_distance_point_plane(plane, vPoint); + char returnvalue = _dis < 0.0f ? 2 : 1; + tparam = -_dis / _dotdir; - if(tparam<tmin) + if (tparam < tmin) { returnvalue = 0; tparam = tmin; } - else if(tparam>tmax) + else if (tparam > tmax) { returnvalue = 0; tparam = tmax; } - pout = tparam*vDir + vPoint; + pout = tparam * vDir + vPoint; return returnvalue; } - //! Find closest points on segments SIMD_FORCE_INLINE void bt_segment_collision( - const btVector3 & vA1, - const btVector3 & vA2, - const btVector3 & vB1, - const btVector3 & vB2, - btVector3 & vPointA, - btVector3 & vPointB) + const btVector3 &vA1, + const btVector3 &vA2, + const btVector3 &vB1, + const btVector3 &vB2, + btVector3 &vPointA, + btVector3 &vPointB) { - btVector3 AD = vA2 - vA1; - btVector3 BD = vB2 - vB1; - btVector3 N = AD.cross(BD); - btScalar tp = N.length2(); - - btVector4 _M;//plane - - if(tp<SIMD_EPSILON)//ARE PARALELE - { - //project B over A - bool invert_b_order = false; - _M[0] = vB1.dot(AD); - _M[1] = vB2.dot(AD); - - if(_M[0]>_M[1]) - { - invert_b_order = true; - BT_SWAP_NUMBERS(_M[0],_M[1]); - } - _M[2] = vA1.dot(AD); - _M[3] = vA2.dot(AD); - //mid points - N[0] = (_M[0]+_M[1])*0.5f; - N[1] = (_M[2]+_M[3])*0.5f; - - if(N[0]<N[1]) - { - if(_M[1]<_M[2]) - { - vPointB = invert_b_order?vB1:vB2; - vPointA = vA1; - } - else if(_M[1]<_M[3]) - { - vPointB = invert_b_order?vB1:vB2; - bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2); - } - else - { - vPointA = vA2; - bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2); - } - } - else - { - if(_M[3]<_M[0]) - { - vPointB = invert_b_order?vB2:vB1; - vPointA = vA2; - } - else if(_M[3]<_M[1]) - { - vPointA = vA2; - bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2); - } - else - { - vPointB = invert_b_order?vB1:vB2; - bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2); - } - } - return; - } - - N = N.cross(BD); - _M.setValue(N[0],N[1],N[2],vB1.dot(N)); + btVector3 AD = vA2 - vA1; + btVector3 BD = vB2 - vB1; + btVector3 N = AD.cross(BD); + btScalar tp = N.length2(); - // get point A as the plane collision point - bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1)); - - /*Closest point on segment*/ - vPointB = vPointA - vB1; - tp = vPointB.dot(BD); - tp/= BD.dot(BD); - tp = BT_CLAMP(tp,0.0f,1.0f); + btVector4 _M; //plane - vPointB = tp*BD + vB1; -} + if (tp < SIMD_EPSILON) //ARE PARALELE + { + //project B over A + bool invert_b_order = false; + _M[0] = vB1.dot(AD); + _M[1] = vB2.dot(AD); + + if (_M[0] > _M[1]) + { + invert_b_order = true; + BT_SWAP_NUMBERS(_M[0], _M[1]); + } + _M[2] = vA1.dot(AD); + _M[3] = vA2.dot(AD); + //mid points + N[0] = (_M[0] + _M[1]) * 0.5f; + N[1] = (_M[2] + _M[3]) * 0.5f; + + if (N[0] < N[1]) + { + if (_M[1] < _M[2]) + { + vPointB = invert_b_order ? vB1 : vB2; + vPointA = vA1; + } + else if (_M[1] < _M[3]) + { + vPointB = invert_b_order ? vB1 : vB2; + bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2); + } + else + { + vPointA = vA2; + bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2); + } + } + else + { + if (_M[3] < _M[0]) + { + vPointB = invert_b_order ? vB2 : vB1; + vPointA = vA2; + } + else if (_M[3] < _M[1]) + { + vPointA = vA2; + bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2); + } + else + { + vPointB = invert_b_order ? vB1 : vB2; + bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2); + } + } + return; + } + N = N.cross(BD); + _M.setValue(N[0], N[1], N[2], vB1.dot(N)); + // get point A as the plane collision point + bt_line_plane_collision(_M, AD, vA1, vPointA, tp, btScalar(0), btScalar(1)); + /*Closest point on segment*/ + vPointB = vPointA - vB1; + tp = vPointB.dot(BD); + tp /= BD.dot(BD); + tp = BT_CLAMP(tp, 0.0f, 1.0f); + vPointB = tp * BD + vB1; +} -#endif // GIM_VECTOR_H_INCLUDED +#endif // GIM_VECTOR_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btQuantization.h b/thirdparty/bullet/BulletCollision/Gimpact/btQuantization.h index bd2633cfc5..19a02a2177 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btQuantization.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btQuantization.h @@ -27,54 +27,47 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" - - - - - SIMD_FORCE_INLINE void bt_calc_quantization_parameters( - btVector3 & outMinBound, - btVector3 & outMaxBound, - btVector3 & bvhQuantization, - const btVector3& srcMinBound,const btVector3& srcMaxBound, + btVector3& outMinBound, + btVector3& outMaxBound, + btVector3& bvhQuantization, + const btVector3& srcMinBound, const btVector3& srcMaxBound, btScalar quantizationMargin) { //enlarge the AABB to avoid division by zero when initializing the quantization values - btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); + btVector3 clampValue(quantizationMargin, quantizationMargin, quantizationMargin); outMinBound = srcMinBound - clampValue; outMaxBound = srcMaxBound + clampValue; btVector3 aabbSize = outMaxBound - outMinBound; bvhQuantization = btVector3(btScalar(65535.0), btScalar(65535.0), - btScalar(65535.0)) / aabbSize; + btScalar(65535.0)) / + aabbSize; } - SIMD_FORCE_INLINE void bt_quantize_clamp( unsigned short* out, const btVector3& point, - const btVector3 & min_bound, - const btVector3 & max_bound, - const btVector3 & bvhQuantization) + const btVector3& min_bound, + const btVector3& max_bound, + const btVector3& bvhQuantization) { - btVector3 clampedPoint(point); clampedPoint.setMax(min_bound); clampedPoint.setMin(max_bound); btVector3 v = (clampedPoint - min_bound) * bvhQuantization; - out[0] = (unsigned short)(v.getX()+0.5f); - out[1] = (unsigned short)(v.getY()+0.5f); - out[2] = (unsigned short)(v.getZ()+0.5f); + out[0] = (unsigned short)(v.getX() + 0.5f); + out[1] = (unsigned short)(v.getY() + 0.5f); + out[2] = (unsigned short)(v.getZ() + 0.5f); } - SIMD_FORCE_INLINE btVector3 bt_unquantize( const unsigned short* vecIn, - const btVector3 & offset, - const btVector3 & bvhQuantization) + const btVector3& offset, + const btVector3& bvhQuantization) { - btVector3 vecOut; + btVector3 vecOut; vecOut.setValue( (btScalar)(vecIn[0]) / (bvhQuantization.getX()), (btScalar)(vecIn[1]) / (bvhQuantization.getY()), @@ -83,6 +76,4 @@ SIMD_FORCE_INLINE btVector3 bt_unquantize( return vecOut; } - - -#endif // BT_GIMPACT_QUANTIZATION_H_INCLUDED +#endif // BT_GIMPACT_QUANTIZATION_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btTriangleShapeEx.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btTriangleShapeEx.cpp index ca76cc54a1..292ef8c1ff 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btTriangleShapeEx.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/btTriangleShapeEx.cpp @@ -23,196 +23,181 @@ subject to the following restrictions: #include "btTriangleShapeEx.h" - - -void GIM_TRIANGLE_CONTACT::merge_points(const btVector4 & plane, - btScalar margin, const btVector3 * points, int point_count) +void GIM_TRIANGLE_CONTACT::merge_points(const btVector4& plane, + btScalar margin, const btVector3* points, int point_count) { - m_point_count = 0; - m_penetration_depth= -1000.0f; + m_point_count = 0; + m_penetration_depth = -1000.0f; - int point_indices[MAX_TRI_CLIPPING]; + int point_indices[MAX_TRI_CLIPPING]; int _k; - for ( _k=0;_k<point_count;_k++) - { - btScalar _dist = - bt_distance_point_plane(plane,points[_k]) + margin; - - if (_dist>=0.0f) - { - if (_dist>m_penetration_depth) - { - m_penetration_depth = _dist; - point_indices[0] = _k; - m_point_count=1; - } - else if ((_dist+SIMD_EPSILON)>=m_penetration_depth) - { - point_indices[m_point_count] = _k; - m_point_count++; - } - } - } - - for ( _k=0;_k<m_point_count;_k++) - { - m_points[_k] = points[point_indices[_k]]; - } + for (_k = 0; _k < point_count; _k++) + { + btScalar _dist = -bt_distance_point_plane(plane, points[_k]) + margin; + + if (_dist >= 0.0f) + { + if (_dist > m_penetration_depth) + { + m_penetration_depth = _dist; + point_indices[0] = _k; + m_point_count = 1; + } + else if ((_dist + SIMD_EPSILON) >= m_penetration_depth) + { + point_indices[m_point_count] = _k; + m_point_count++; + } + } + } + + for (_k = 0; _k < m_point_count; _k++) + { + m_points[_k] = points[point_indices[_k]]; + } } ///class btPrimitiveTriangle bool btPrimitiveTriangle::overlap_test_conservative(const btPrimitiveTriangle& other) { - btScalar total_margin = m_margin + other.m_margin; - // classify points on other triangle - btScalar dis0 = bt_distance_point_plane(m_plane,other.m_vertices[0]) - total_margin; + btScalar total_margin = m_margin + other.m_margin; + // classify points on other triangle + btScalar dis0 = bt_distance_point_plane(m_plane, other.m_vertices[0]) - total_margin; - btScalar dis1 = bt_distance_point_plane(m_plane,other.m_vertices[1]) - total_margin; + btScalar dis1 = bt_distance_point_plane(m_plane, other.m_vertices[1]) - total_margin; - btScalar dis2 = bt_distance_point_plane(m_plane,other.m_vertices[2]) - total_margin; + btScalar dis2 = bt_distance_point_plane(m_plane, other.m_vertices[2]) - total_margin; - if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false; - // classify points on this triangle - dis0 = bt_distance_point_plane(other.m_plane,m_vertices[0]) - total_margin; + // classify points on this triangle + dis0 = bt_distance_point_plane(other.m_plane, m_vertices[0]) - total_margin; - dis1 = bt_distance_point_plane(other.m_plane,m_vertices[1]) - total_margin; + dis1 = bt_distance_point_plane(other.m_plane, m_vertices[1]) - total_margin; - dis2 = bt_distance_point_plane(other.m_plane,m_vertices[2]) - total_margin; + dis2 = bt_distance_point_plane(other.m_plane, m_vertices[2]) - total_margin; - if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false; - return true; + return true; } -int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points ) +int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle& other, btVector3* clipped_points) { - // edge 0 - - btVector3 temp_points[MAX_TRI_CLIPPING]; - + // edge 0 - btVector4 edgeplane; + btVector3 temp_points[MAX_TRI_CLIPPING]; - get_edge_plane(0,edgeplane); + btVector4 edgeplane; + get_edge_plane(0, edgeplane); - int clipped_count = bt_plane_clip_triangle( - edgeplane,other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],temp_points); + int clipped_count = bt_plane_clip_triangle( + edgeplane, other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], temp_points); - if (clipped_count == 0) return 0; + if (clipped_count == 0) return 0; - btVector3 temp_points1[MAX_TRI_CLIPPING]; + btVector3 temp_points1[MAX_TRI_CLIPPING]; + // edge 1 + get_edge_plane(1, edgeplane); - // edge 1 - get_edge_plane(1,edgeplane); + clipped_count = bt_plane_clip_polygon(edgeplane, temp_points, clipped_count, temp_points1); + if (clipped_count == 0) return 0; - clipped_count = bt_plane_clip_polygon(edgeplane,temp_points,clipped_count,temp_points1); + // edge 2 + get_edge_plane(2, edgeplane); - if (clipped_count == 0) return 0; + clipped_count = bt_plane_clip_polygon( + edgeplane, temp_points1, clipped_count, clipped_points); - // edge 2 - get_edge_plane(2,edgeplane); - - clipped_count = bt_plane_clip_polygon( - edgeplane,temp_points1,clipped_count,clipped_points); - - return clipped_count; + return clipped_count; } -bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts) +bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_TRIANGLE_CONTACT& contacts) { - btScalar margin = m_margin + other.m_margin; - - btVector3 clipped_points[MAX_TRI_CLIPPING]; - int clipped_count; - //create planes - // plane v vs U points - - GIM_TRIANGLE_CONTACT contacts1; - - contacts1.m_separating_normal = m_plane; - - - clipped_count = clip_triangle(other,clipped_points); - - if (clipped_count == 0 ) - { - return false;//Reject - } - - //find most deep interval face1 - contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count); - if (contacts1.m_point_count == 0) return false; // too far - //Normal pointing to this triangle - contacts1.m_separating_normal *= -1.f; - - - //Clip tri1 by tri2 edges - GIM_TRIANGLE_CONTACT contacts2; - contacts2.m_separating_normal = other.m_plane; - - clipped_count = other.clip_triangle(*this,clipped_points); - - if (clipped_count == 0 ) - { - return false;//Reject - } - - //find most deep interval face1 - contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count); - if (contacts2.m_point_count == 0) return false; // too far - - - - - ////check most dir for contacts - if (contacts2.m_penetration_depth<contacts1.m_penetration_depth) - { - contacts.copy_from(contacts2); - } - else - { - contacts.copy_from(contacts1); - } - return true; + btScalar margin = m_margin + other.m_margin; + + btVector3 clipped_points[MAX_TRI_CLIPPING]; + int clipped_count; + //create planes + // plane v vs U points + + GIM_TRIANGLE_CONTACT contacts1; + + contacts1.m_separating_normal = m_plane; + + clipped_count = clip_triangle(other, clipped_points); + + if (clipped_count == 0) + { + return false; //Reject + } + + //find most deep interval face1 + contacts1.merge_points(contacts1.m_separating_normal, margin, clipped_points, clipped_count); + if (contacts1.m_point_count == 0) return false; // too far + //Normal pointing to this triangle + contacts1.m_separating_normal *= -1.f; + + //Clip tri1 by tri2 edges + GIM_TRIANGLE_CONTACT contacts2; + contacts2.m_separating_normal = other.m_plane; + + clipped_count = other.clip_triangle(*this, clipped_points); + + if (clipped_count == 0) + { + return false; //Reject + } + + //find most deep interval face1 + contacts2.merge_points(contacts2.m_separating_normal, margin, clipped_points, clipped_count); + if (contacts2.m_point_count == 0) return false; // too far + + ////check most dir for contacts + if (contacts2.m_penetration_depth < contacts1.m_penetration_depth) + { + contacts.copy_from(contacts2); + } + else + { + contacts.copy_from(contacts1); + } + return true; } - - ///class btTriangleShapeEx: public btTriangleShape bool btTriangleShapeEx::overlap_test_conservative(const btTriangleShapeEx& other) { - btScalar total_margin = getMargin() + other.getMargin(); + btScalar total_margin = getMargin() + other.getMargin(); - btVector4 plane0; - buildTriPlane(plane0); - btVector4 plane1; - other.buildTriPlane(plane1); + btVector4 plane0; + buildTriPlane(plane0); + btVector4 plane1; + other.buildTriPlane(plane1); - // classify points on other triangle - btScalar dis0 = bt_distance_point_plane(plane0,other.m_vertices1[0]) - total_margin; + // classify points on other triangle + btScalar dis0 = bt_distance_point_plane(plane0, other.m_vertices1[0]) - total_margin; - btScalar dis1 = bt_distance_point_plane(plane0,other.m_vertices1[1]) - total_margin; + btScalar dis1 = bt_distance_point_plane(plane0, other.m_vertices1[1]) - total_margin; - btScalar dis2 = bt_distance_point_plane(plane0,other.m_vertices1[2]) - total_margin; + btScalar dis2 = bt_distance_point_plane(plane0, other.m_vertices1[2]) - total_margin; - if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false; - // classify points on this triangle - dis0 = bt_distance_point_plane(plane1,m_vertices1[0]) - total_margin; + // classify points on this triangle + dis0 = bt_distance_point_plane(plane1, m_vertices1[0]) - total_margin; - dis1 = bt_distance_point_plane(plane1,m_vertices1[1]) - total_margin; + dis1 = bt_distance_point_plane(plane1, m_vertices1[1]) - total_margin; - dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - total_margin; + dis2 = bt_distance_point_plane(plane1, m_vertices1[2]) - total_margin; - if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false; - return true; + return true; } - - diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btTriangleShapeEx.h b/thirdparty/bullet/BulletCollision/Gimpact/btTriangleShapeEx.h index 973c2ed127..568a1ce811 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btTriangleShapeEx.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btTriangleShapeEx.h @@ -21,7 +21,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef GIMPACT_TRIANGLE_SHAPE_EX_H #define GIMPACT_TRIANGLE_SHAPE_EX_H @@ -31,16 +30,15 @@ subject to the following restrictions: #include "btClipPolygon.h" #include "btGeometryOperations.h" - #define MAX_TRI_CLIPPING 16 //! Structure for collision struct GIM_TRIANGLE_CONTACT { - btScalar m_penetration_depth; - int m_point_count; - btVector4 m_separating_normal; - btVector3 m_points[MAX_TRI_CLIPPING]; + btScalar m_penetration_depth; + int m_point_count; + btVector4 m_separating_normal; + btVector3 m_points[MAX_TRI_CLIPPING]; SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT& other) { @@ -48,7 +46,7 @@ struct GIM_TRIANGLE_CONTACT m_separating_normal = other.m_separating_normal; m_point_count = other.m_point_count; int i = m_point_count; - while(i--) + while (i--) { m_points[i] = other.m_points[i]; } @@ -63,14 +61,11 @@ struct GIM_TRIANGLE_CONTACT copy_from(other); } - //! classify points that are closer - void merge_points(const btVector4 & plane, - btScalar margin, const btVector3 * points, int point_count); - + //! classify points that are closer + void merge_points(const btVector4& plane, + btScalar margin, const btVector3* points, int point_count); }; - - class btPrimitiveTriangle { public: @@ -78,17 +73,15 @@ public: btVector4 m_plane; btScalar m_margin; btScalar m_dummy; - btPrimitiveTriangle():m_margin(0.01f) + btPrimitiveTriangle() : m_margin(0.01f) { - } - SIMD_FORCE_INLINE void buildTriPlane() { - btVector3 normal = (m_vertices[1]-m_vertices[0]).cross(m_vertices[2]-m_vertices[0]); + btVector3 normal = (m_vertices[1] - m_vertices[0]).cross(m_vertices[2] - m_vertices[0]); normal.normalize(); - m_plane.setValue(normal[0],normal[1],normal[2],m_vertices[0].dot(normal)); + m_plane.setValue(normal[0], normal[1], normal[2], m_vertices[0].dot(normal)); } //! Test if triangles could collide @@ -98,14 +91,14 @@ public: /*! \pre this triangle must have its plane calculated. */ - SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4 &plane) const - { - const btVector3 & e0 = m_vertices[edge_index]; - const btVector3 & e1 = m_vertices[(edge_index+1)%3]; - bt_edge_plane(e0,e1,m_plane,plane); - } - - void applyTransform(const btTransform& t) + SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4& plane) const + { + const btVector3& e0 = m_vertices[edge_index]; + const btVector3& e1 = m_vertices[(edge_index + 1) % 3]; + bt_edge_plane(e0, e1, m_plane, plane); + } + + void applyTransform(const btTransform& t) { m_vertices[0] = t(m_vertices[0]); m_vertices[1] = t(m_vertices[1]); @@ -117,44 +110,41 @@ public: \pre clipped_points must have MAX_TRI_CLIPPING size, and this triangle must have its plane calculated. \return the number of clipped points */ - int clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points ); + int clip_triangle(btPrimitiveTriangle& other, btVector3* clipped_points); //! Find collision using the clipping method /*! \pre this triangle and other must have their triangles calculated */ - bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts); + bool find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_TRIANGLE_CONTACT& contacts); }; - - //! Helper class for colliding Bullet Triangle Shapes /*! This class implements a better getAabb method than the previous btTriangleShape class */ -class btTriangleShapeEx: public btTriangleShape +class btTriangleShapeEx : public btTriangleShape { public: - - btTriangleShapeEx():btTriangleShape(btVector3(0,0,0),btVector3(0,0,0),btVector3(0,0,0)) + btTriangleShapeEx() : btTriangleShape(btVector3(0, 0, 0), btVector3(0, 0, 0), btVector3(0, 0, 0)) { } - btTriangleShapeEx(const btVector3& p0,const btVector3& p1,const btVector3& p2): btTriangleShape(p0,p1,p2) + btTriangleShapeEx(const btVector3& p0, const btVector3& p1, const btVector3& p2) : btTriangleShape(p0, p1, p2) { } - btTriangleShapeEx(const btTriangleShapeEx & other): btTriangleShape(other.m_vertices1[0],other.m_vertices1[1],other.m_vertices1[2]) + btTriangleShapeEx(const btTriangleShapeEx& other) : btTriangleShape(other.m_vertices1[0], other.m_vertices1[1], other.m_vertices1[2]) { } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { btVector3 tv0 = t(m_vertices1[0]); btVector3 tv1 = t(m_vertices1[1]); btVector3 tv2 = t(m_vertices1[2]); - btAABB trianglebox(tv0,tv1,tv2,m_collisionMargin); + btAABB trianglebox(tv0, tv1, tv2, m_collisionMargin); aabbMin = trianglebox.m_min; aabbMax = trianglebox.m_max; } @@ -166,15 +156,14 @@ public: m_vertices1[2] = t(m_vertices1[2]); } - SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const + SIMD_FORCE_INLINE void buildTriPlane(btVector4& plane) const { - btVector3 normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]); + btVector3 normal = (m_vertices1[1] - m_vertices1[0]).cross(m_vertices1[2] - m_vertices1[0]); normal.normalize(); - plane.setValue(normal[0],normal[1],normal[2],m_vertices1[0].dot(normal)); + plane.setValue(normal[0], normal[1], normal[2], m_vertices1[0].dot(normal)); } bool overlap_test_conservative(const btTriangleShapeEx& other); }; - -#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H +#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_array.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_array.h index cda51a5fce..fc2dc38a33 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_array.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_array.h @@ -34,47 +34,46 @@ email: projectileman@yahoo.com #include "gim_memory.h" - #define GIM_ARRAY_GROW_INCREMENT 2 #define GIM_ARRAY_GROW_FACTOR 2 //! Very simple array container with fast access and simd memory -template<typename T> +template <typename T> class gim_array { public: -//! properties -//!@{ - T *m_data; - GUINT m_size; - GUINT m_allocated_size; -//!@} -//! protected operations -//!@{ - - inline void destroyData() - { - m_allocated_size = 0; - if(m_data==NULL) return; + //! properties + //!@{ + T* m_data; + GUINT m_size; + GUINT m_allocated_size; + //!@} + //! protected operations + //!@{ + + inline void destroyData() + { + m_allocated_size = 0; + if (m_data == NULL) return; gim_free(m_data); m_data = NULL; } inline bool resizeData(GUINT newsize) { - if(newsize==0) + if (newsize == 0) { destroyData(); return true; } - if(m_size>0) + if (m_size > 0) { - m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T)); + m_data = (T*)gim_realloc(m_data, m_size * sizeof(T), newsize * sizeof(T)); } else { - m_data = (T*)gim_alloc(newsize*sizeof(T)); + m_data = (T*)gim_alloc(newsize * sizeof(T)); } m_allocated_size = newsize; return true; @@ -82,243 +81,238 @@ public: inline bool growingCheck() { - if(m_allocated_size<=m_size) + if (m_allocated_size <= m_size) { - GUINT requestsize = m_size; - m_size = m_allocated_size; - if(resizeData((requestsize+GIM_ARRAY_GROW_INCREMENT)*GIM_ARRAY_GROW_FACTOR)==false) return false; + GUINT requestsize = m_size; + m_size = m_allocated_size; + if (resizeData((requestsize + GIM_ARRAY_GROW_INCREMENT) * GIM_ARRAY_GROW_FACTOR) == false) return false; } return true; } -//!@} -//! public operations -//!@{ - inline bool reserve(GUINT size) - { - if(m_allocated_size>=size) return false; - return resizeData(size); - } - - inline void clear_range(GUINT start_range) - { - while(m_size>start_range) - { - m_data[--m_size].~T(); - } - } - - inline void clear() - { - if(m_size==0)return; - clear_range(0); - } - - inline void clear_memory() - { - clear(); - destroyData(); - } - - gim_array() - { - m_data = 0; - m_size = 0; - m_allocated_size = 0; - } - - gim_array(GUINT reservesize) - { - m_data = 0; - m_size = 0; - - m_allocated_size = 0; - reserve(reservesize); - } - - ~gim_array() - { - clear_memory(); - } - - inline GUINT size() const - { - return m_size; - } - - inline GUINT max_size() const - { - return m_allocated_size; - } - - inline T & operator[](size_t i) + //!@} + //! public operations + //!@{ + inline bool reserve(GUINT size) + { + if (m_allocated_size >= size) return false; + return resizeData(size); + } + + inline void clear_range(GUINT start_range) + { + while (m_size > start_range) + { + m_data[--m_size].~T(); + } + } + + inline void clear() + { + if (m_size == 0) return; + clear_range(0); + } + + inline void clear_memory() + { + clear(); + destroyData(); + } + + gim_array() + { + m_data = 0; + m_size = 0; + m_allocated_size = 0; + } + + gim_array(GUINT reservesize) + { + m_data = 0; + m_size = 0; + + m_allocated_size = 0; + reserve(reservesize); + } + + ~gim_array() + { + clear_memory(); + } + + inline GUINT size() const + { + return m_size; + } + + inline GUINT max_size() const + { + return m_allocated_size; + } + + inline T& operator[](size_t i) { return m_data[i]; } - inline const T & operator[](size_t i) const + inline const T& operator[](size_t i) const { return m_data[i]; } - inline T * pointer(){ return m_data;} - inline const T * pointer() const - { return m_data;} - + inline T* pointer() { return m_data; } + inline const T* pointer() const + { + return m_data; + } - inline T * get_pointer_at(GUINT i) + inline T* get_pointer_at(GUINT i) { return m_data + i; } - inline const T * get_pointer_at(GUINT i) const + inline const T* get_pointer_at(GUINT i) const { return m_data + i; } - inline T & at(GUINT i) + inline T& at(GUINT i) { return m_data[i]; } - inline const T & at(GUINT i) const + inline const T& at(GUINT i) const { return m_data[i]; } - inline T & front() + inline T& front() { return *m_data; } - inline const T & front() const + inline const T& front() const { return *m_data; } - inline T & back() + inline T& back() { - return m_data[m_size-1]; + return m_data[m_size - 1]; } - inline const T & back() const + inline const T& back() const { - return m_data[m_size-1]; + return m_data[m_size - 1]; } - inline void swap(GUINT i, GUINT j) { - gim_swap_elements(m_data,i,j); + gim_swap_elements(m_data, i, j); } - inline void push_back(const T & obj) + inline void push_back(const T& obj) { - this->growingCheck(); - m_data[m_size] = obj; - m_size++; + this->growingCheck(); + m_data[m_size] = obj; + m_size++; } //!Simply increase the m_size, doesn't call the new element constructor inline void push_back_mem() { - this->growingCheck(); - m_size++; + this->growingCheck(); + m_size++; } - inline void push_back_memcpy(const T & obj) + inline void push_back_memcpy(const T& obj) { - this->growingCheck(); - gim_simd_memcpy(&m_data[m_size],&obj,sizeof(T)); - m_size++; + this->growingCheck(); + gim_simd_memcpy(&m_data[m_size], &obj, sizeof(T)); + m_size++; } inline void pop_back() { - m_size--; - m_data[m_size].~T(); + m_size--; + m_data[m_size].~T(); } //!Simply decrease the m_size, doesn't call the deleted element destructor inline void pop_back_mem() { - m_size--; + m_size--; } - //! fast erase + //! fast erase inline void erase(GUINT index) { - if(index<m_size-1) - { - swap(index,m_size-1); - } - pop_back(); + if (index < m_size - 1) + { + swap(index, m_size - 1); + } + pop_back(); } inline void erase_sorted_mem(GUINT index) { - m_size--; - for(GUINT i = index;i<m_size;i++) - { - gim_simd_memcpy(m_data+i,m_data+i+1,sizeof(T)); - } + m_size--; + for (GUINT i = index; i < m_size; i++) + { + gim_simd_memcpy(m_data + i, m_data + i + 1, sizeof(T)); + } } inline void erase_sorted(GUINT index) { - m_data[index].~T(); - erase_sorted_mem(index); + m_data[index].~T(); + erase_sorted_mem(index); } inline void insert_mem(GUINT index) { - this->growingCheck(); - for(GUINT i = m_size;i>index;i--) - { - gim_simd_memcpy(m_data+i,m_data+i-1,sizeof(T)); - } - m_size++; - } - - inline void insert(const T & obj,GUINT index) - { - insert_mem(index); - m_data[index] = obj; - } - - inline void resize(GUINT size, bool call_constructor = true, const T& fillData=T()) - { - if(size>m_size) - { - reserve(size); - if(call_constructor) - { - while(m_size<size) - { - m_data[m_size] = fillData; - m_size++; - } - } - else - { - m_size = size; - } - } - else if(size<m_size) - { - if(call_constructor) clear_range(size); - m_size = size; - } + this->growingCheck(); + for (GUINT i = m_size; i > index; i--) + { + gim_simd_memcpy(m_data + i, m_data + i - 1, sizeof(T)); + } + m_size++; } - inline void refit() + inline void insert(const T& obj, GUINT index) { - resizeData(m_size); + insert_mem(index); + m_data[index] = obj; } -}; - - - + inline void resize(GUINT size, bool call_constructor = true, const T& fillData = T()) + { + if (size > m_size) + { + reserve(size); + if (call_constructor) + { + while (m_size < size) + { + m_data[m_size] = fillData; + m_size++; + } + } + else + { + m_size = size; + } + } + else if (size < m_size) + { + if (call_constructor) clear_range(size); + m_size = size; + } + } + inline void refit() + { + resizeData(m_size); + } +}; -#endif // GIM_CONTAINERS_H_INCLUDED +#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_basic_geometry_operations.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_basic_geometry_operations.h index 0c48cb60fc..7ab783672d 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_basic_geometry_operations.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_basic_geometry_operations.h @@ -35,12 +35,8 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_linear_math.h" - - - #ifndef PLANEDIREPSILON #define PLANEDIREPSILON 0.0000001f #endif @@ -49,77 +45,82 @@ email: projectileman@yahoo.com #define PARALELENORMALS 0.000001f #endif -#define TRIANGLE_NORMAL(v1,v2,v3,n)\ -{\ - vec3f _dif1,_dif2;\ - VEC_DIFF(_dif1,v2,v1);\ - VEC_DIFF(_dif2,v3,v1);\ - VEC_CROSS(n,_dif1,_dif2);\ - VEC_NORMALIZE(n);\ -}\ - -#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\ - vec3f _dif1,_dif2; \ - VEC_DIFF(_dif1,v2,v1); \ - VEC_DIFF(_dif2,v3,v1); \ - VEC_CROSS(n,_dif1,_dif2); \ -}\ +#define TRIANGLE_NORMAL(v1, v2, v3, n) \ + { \ + vec3f _dif1, _dif2; \ + VEC_DIFF(_dif1, v2, v1); \ + VEC_DIFF(_dif2, v3, v1); \ + VEC_CROSS(n, _dif1, _dif2); \ + VEC_NORMALIZE(n); \ + } + +#define TRIANGLE_NORMAL_FAST(v1, v2, v3, n) \ + { \ + vec3f _dif1, _dif2; \ + VEC_DIFF(_dif1, v2, v1); \ + VEC_DIFF(_dif2, v3, v1); \ + VEC_CROSS(n, _dif1, _dif2); \ + } /// plane is a vec4f -#define TRIANGLE_PLANE(v1,v2,v3,plane) {\ - TRIANGLE_NORMAL(v1,v2,v3,plane);\ - plane[3] = VEC_DOT(v1,plane);\ -}\ +#define TRIANGLE_PLANE(v1, v2, v3, plane) \ + { \ + TRIANGLE_NORMAL(v1, v2, v3, plane); \ + plane[3] = VEC_DOT(v1, plane); \ + } /// plane is a vec4f -#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\ - TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\ - plane[3] = VEC_DOT(v1,plane);\ -}\ +#define TRIANGLE_PLANE_FAST(v1, v2, v3, plane) \ + { \ + TRIANGLE_NORMAL_FAST(v1, v2, v3, plane); \ + plane[3] = VEC_DOT(v1, plane); \ + } /// Calc a plane from an edge an a normal. plane is a vec4f -#define EDGE_PLANE(e1,e2,n,plane) {\ - vec3f _dif; \ - VEC_DIFF(_dif,e2,e1); \ - VEC_CROSS(plane,_dif,n); \ - VEC_NORMALIZE(plane); \ - plane[3] = VEC_DOT(e1,plane);\ -}\ - -#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3]) - -#define PROJECT_POINT_PLANE(point,plane,projected) {\ - GREAL _dis;\ - _dis = DISTANCE_PLANE_POINT(plane,point);\ - VEC_SCALE(projected,-_dis,plane);\ - VEC_SUM(projected,projected,point); \ -}\ +#define EDGE_PLANE(e1, e2, n, plane) \ + { \ + vec3f _dif; \ + VEC_DIFF(_dif, e2, e1); \ + VEC_CROSS(plane, _dif, n); \ + VEC_NORMALIZE(plane); \ + plane[3] = VEC_DOT(e1, plane); \ + } + +#define DISTANCE_PLANE_POINT(plane, point) (VEC_DOT(plane, point) - plane[3]) + +#define PROJECT_POINT_PLANE(point, plane, projected) \ + { \ + GREAL _dis; \ + _dis = DISTANCE_PLANE_POINT(plane, point); \ + VEC_SCALE(projected, -_dis, plane); \ + VEC_SUM(projected, projected, point); \ + } //! Verifies if a point is in the plane hull -template<typename CLASS_POINT,typename CLASS_PLANE> +template <typename CLASS_POINT, typename CLASS_PLANE> SIMD_FORCE_INLINE bool POINT_IN_HULL( - const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count) + const CLASS_POINT &point, const CLASS_PLANE *planes, GUINT plane_count) { GREAL _dis; - for (GUINT _i = 0;_i< plane_count;++_i) + for (GUINT _i = 0; _i < plane_count; ++_i) { - _dis = DISTANCE_PLANE_POINT(planes[_i],point); - if(_dis>0.0f) return false; + _dis = DISTANCE_PLANE_POINT(planes[_i], point); + if (_dis > 0.0f) return false; } return true; } -template<typename CLASS_POINT,typename CLASS_PLANE> +template <typename CLASS_POINT, typename CLASS_PLANE> SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT( - const CLASS_POINT& s1, - const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped) + const CLASS_POINT &s1, + const CLASS_POINT &s2, const CLASS_PLANE &plane, CLASS_POINT &clipped) { - GREAL _dis1,_dis2; - _dis1 = DISTANCE_PLANE_POINT(plane,s1); - VEC_DIFF(clipped,s2,s1); - _dis2 = VEC_DOT(clipped,plane); - VEC_SCALE(clipped,-_dis1/_dis2,clipped); - VEC_SUM(clipped,clipped,s1); + GREAL _dis1, _dis2; + _dis1 = DISTANCE_PLANE_POINT(plane, s1); + VEC_DIFF(clipped, s2, s1); + _dis2 = VEC_DOT(clipped, plane); + VEC_SCALE(clipped, -_dis1 / _dis2, clipped); + VEC_SUM(clipped, clipped, s1); } enum ePLANE_INTERSECTION_TYPE @@ -152,30 +153,30 @@ intersection type must have the following values </ul> */ -template<typename CLASS_POINT,typename CLASS_PLANE> +template <typename CLASS_POINT, typename CLASS_PLANE> SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2( - const CLASS_POINT& s1, + const CLASS_POINT &s1, const CLASS_POINT &s2, - const CLASS_PLANE &plane,CLASS_POINT &clipped) + const CLASS_PLANE &plane, CLASS_POINT &clipped) { - GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1); - GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2); - if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON) + GREAL _dis1 = DISTANCE_PLANE_POINT(plane, s1); + GREAL _dis2 = DISTANCE_PLANE_POINT(plane, s2); + if (_dis1 > -G_EPSILON && _dis2 > -G_EPSILON) { - if(_dis1<_dis2) return G_FRONT_PLANE_S1; - return G_FRONT_PLANE_S2; + if (_dis1 < _dis2) return G_FRONT_PLANE_S1; + return G_FRONT_PLANE_S2; } - else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON) + else if (_dis1 < G_EPSILON && _dis2 < G_EPSILON) { - if(_dis1>_dis2) return G_BACK_PLANE_S1; - return G_BACK_PLANE_S2; + if (_dis1 > _dis2) return G_BACK_PLANE_S1; + return G_BACK_PLANE_S2; } - VEC_DIFF(clipped,s2,s1); - _dis2 = VEC_DOT(clipped,plane); - VEC_SCALE(clipped,-_dis1/_dis2,clipped); - VEC_SUM(clipped,clipped,s1); - if(_dis1<_dis2) return G_COLLIDE_PLANE_S1; + VEC_DIFF(clipped, s2, s1); + _dis2 = VEC_DOT(clipped, plane); + VEC_SCALE(clipped, -_dis1 / _dis2, clipped); + VEC_SUM(clipped, clipped, s1); + if (_dis1 < _dis2) return G_COLLIDE_PLANE_S1; return G_COLLIDE_PLANE_S2; } @@ -194,43 +195,42 @@ intersection_type must have the following values <li> 5 : Segment collides plane, s2 in back </ul> */ -template<typename CLASS_POINT,typename CLASS_PLANE> +template <typename CLASS_POINT, typename CLASS_PLANE> SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST( - const CLASS_POINT& s1, + const CLASS_POINT &s1, const CLASS_POINT &s2, const CLASS_PLANE &plane, - CLASS_POINT &clipped1,CLASS_POINT &clipped2) + CLASS_POINT &clipped1, CLASS_POINT &clipped2) { - eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1); - switch(intersection_type) + eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1, s2, plane, clipped1); + switch (intersection_type) { - case G_FRONT_PLANE_S1: - VEC_COPY(clipped1,s1); - VEC_COPY(clipped2,s2); - break; - case G_FRONT_PLANE_S2: - VEC_COPY(clipped1,s2); - VEC_COPY(clipped2,s1); - break; - case G_BACK_PLANE_S1: - VEC_COPY(clipped1,s1); - VEC_COPY(clipped2,s2); - break; - case G_BACK_PLANE_S2: - VEC_COPY(clipped1,s2); - VEC_COPY(clipped2,s1); - break; - case G_COLLIDE_PLANE_S1: - VEC_COPY(clipped2,s1); - break; - case G_COLLIDE_PLANE_S2: - VEC_COPY(clipped2,s2); - break; + case G_FRONT_PLANE_S1: + VEC_COPY(clipped1, s1); + VEC_COPY(clipped2, s2); + break; + case G_FRONT_PLANE_S2: + VEC_COPY(clipped1, s2); + VEC_COPY(clipped2, s1); + break; + case G_BACK_PLANE_S1: + VEC_COPY(clipped1, s1); + VEC_COPY(clipped2, s2); + break; + case G_BACK_PLANE_S2: + VEC_COPY(clipped1, s2); + VEC_COPY(clipped2, s1); + break; + case G_COLLIDE_PLANE_S1: + VEC_COPY(clipped2, s1); + break; + case G_COLLIDE_PLANE_S2: + VEC_COPY(clipped2, s2); + break; } return intersection_type; } - //! Finds the 2 smallest cartesian coordinates of a plane normal #define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1) @@ -239,23 +239,23 @@ SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST( Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/> It uses the PLANEDIREPSILON constant. */ -template<typename T,typename CLASS_POINT,typename CLASS_PLANE> +template <typename T, typename CLASS_POINT, typename CLASS_PLANE> SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION( - const CLASS_PLANE & plane, - const CLASS_POINT & vDir, - const CLASS_POINT & vPoint, - CLASS_POINT & pout,T &tparam) + const CLASS_PLANE &plane, + const CLASS_POINT &vDir, + const CLASS_POINT &vPoint, + CLASS_POINT &pout, T &tparam) { - GREAL _dis,_dotdir; - _dotdir = VEC_DOT(plane,vDir); - if(_dotdir<PLANEDIREPSILON) + GREAL _dis, _dotdir; + _dotdir = VEC_DOT(plane, vDir); + if (_dotdir < PLANEDIREPSILON) { - return false; + return false; } - _dis = DISTANCE_PLANE_POINT(plane,vPoint); - tparam = -_dis/_dotdir; - VEC_SCALE(pout,tparam,vDir); - VEC_SUM(pout,vPoint,pout); + _dis = DISTANCE_PLANE_POINT(plane, vPoint); + tparam = -_dis / _dotdir; + VEC_SCALE(pout, tparam, vDir); + VEC_SUM(pout, vPoint, pout); return true; } @@ -266,39 +266,39 @@ SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION( -1 if the ray collides in front -2 if the ray collides in back */ -template<typename T,typename CLASS_POINT,typename CLASS_PLANE> +template <typename T, typename CLASS_POINT, typename CLASS_PLANE> SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION( - const CLASS_PLANE & plane, - const CLASS_POINT & vDir, - const CLASS_POINT & vPoint, - CLASS_POINT & pout, + const CLASS_PLANE &plane, + const CLASS_POINT &vDir, + const CLASS_POINT &vPoint, + CLASS_POINT &pout, T &tparam, T tmin, T tmax) { - GREAL _dis,_dotdir; - _dotdir = VEC_DOT(plane,vDir); - if(btFabs(_dotdir)<PLANEDIREPSILON) + GREAL _dis, _dotdir; + _dotdir = VEC_DOT(plane, vDir); + if (btFabs(_dotdir) < PLANEDIREPSILON) { tparam = tmax; - return 0; + return 0; } - _dis = DISTANCE_PLANE_POINT(plane,vPoint); - char returnvalue = _dis<0.0f?2:1; - tparam = -_dis/_dotdir; + _dis = DISTANCE_PLANE_POINT(plane, vPoint); + char returnvalue = _dis < 0.0f ? 2 : 1; + tparam = -_dis / _dotdir; - if(tparam<tmin) + if (tparam < tmin) { returnvalue = 0; tparam = tmin; } - else if(tparam>tmax) + else if (tparam > tmax) { returnvalue = 0; tparam = tmax; } - VEC_SCALE(pout,tparam,vDir); - VEC_SUM(pout,vPoint,pout); + VEC_SCALE(pout, tparam, vDir); + VEC_SUM(pout, vPoint, pout); return returnvalue; } @@ -312,24 +312,24 @@ SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION( \return true if the planes intersect, 0 if paralell. */ -template<typename CLASS_POINT,typename CLASS_PLANE> +template <typename CLASS_POINT, typename CLASS_PLANE> SIMD_FORCE_INLINE bool INTERSECT_PLANES( - const CLASS_PLANE &p1, - const CLASS_PLANE &p2, - CLASS_POINT &p, - CLASS_POINT &d) + const CLASS_PLANE &p1, + const CLASS_PLANE &p2, + CLASS_POINT &p, + CLASS_POINT &d) { - VEC_CROSS(d,p1,p2); - GREAL denom = VEC_DOT(d, d); - if(GIM_IS_ZERO(denom)) return false; + VEC_CROSS(d, p1, p2); + GREAL denom = VEC_DOT(d, d); + if (GIM_IS_ZERO(denom)) return false; vec3f _n; - _n[0]=p1[3]*p2[0] - p2[3]*p1[0]; - _n[1]=p1[3]*p2[1] - p2[3]*p1[1]; - _n[2]=p1[3]*p2[2] - p2[3]*p1[2]; - VEC_CROSS(p,_n,d); - p[0]/=denom; - p[1]/=denom; - p[2]/=denom; + _n[0] = p1[3] * p2[0] - p2[3] * p1[0]; + _n[1] = p1[3] * p2[1] - p2[3] * p1[1]; + _n[2] = p1[3] * p2[2] - p2[3] * p1[2]; + VEC_CROSS(p, _n, d); + p[0] /= denom; + p[1] /= denom; + p[2] /= denom; return true; } @@ -337,32 +337,31 @@ SIMD_FORCE_INLINE bool INTERSECT_PLANES( /*! Finds the closest point(cp) to (v) on a segment (e1,e2) */ -template<typename CLASS_POINT> +template <typename CLASS_POINT> SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT( - CLASS_POINT & cp, const CLASS_POINT & v, - const CLASS_POINT &e1,const CLASS_POINT &e2) + CLASS_POINT &cp, const CLASS_POINT &v, + const CLASS_POINT &e1, const CLASS_POINT &e2) { - vec3f _n; - VEC_DIFF(_n,e2,e1); - VEC_DIFF(cp,v,e1); + vec3f _n; + VEC_DIFF(_n, e2, e1); + VEC_DIFF(cp, v, e1); GREAL _scalar = VEC_DOT(cp, _n); - _scalar/= VEC_DOT(_n, _n); - if(_scalar <0.0f) + _scalar /= VEC_DOT(_n, _n); + if (_scalar < 0.0f) { - VEC_COPY(cp,e1); + VEC_COPY(cp, e1); } - else if(_scalar >1.0f) + else if (_scalar > 1.0f) { - VEC_COPY(cp,e2); + VEC_COPY(cp, e2); } else { - VEC_SCALE(cp,_scalar,_n); - VEC_SUM(cp,cp,e1); + VEC_SCALE(cp, _scalar, _n); + VEC_SUM(cp, cp, e1); } } - /*! \brief Finds the line params where these lines intersect. \param dir1 Direction of line 1 @@ -374,117 +373,113 @@ SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT( \param dointersect 0 if the lines won't intersect, else 1 */ -template<typename T,typename CLASS_POINT> +template <typename T, typename CLASS_POINT> SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS( - const CLASS_POINT & dir1, - CLASS_POINT & point1, - const CLASS_POINT & dir2, - CLASS_POINT & point2, - T& t1,T& t2) + const CLASS_POINT &dir1, + CLASS_POINT &point1, + const CLASS_POINT &dir2, + CLASS_POINT &point2, + T &t1, T &t2) { - GREAL det; - GREAL e1e1 = VEC_DOT(dir1,dir1); - GREAL e1e2 = VEC_DOT(dir1,dir2); - GREAL e2e2 = VEC_DOT(dir2,dir2); + GREAL det; + GREAL e1e1 = VEC_DOT(dir1, dir1); + GREAL e1e2 = VEC_DOT(dir1, dir2); + GREAL e2e2 = VEC_DOT(dir2, dir2); vec3f p1p2; - VEC_DIFF(p1p2,point1,point2); - GREAL p1p2e1 = VEC_DOT(p1p2,dir1); - GREAL p1p2e2 = VEC_DOT(p1p2,dir2); - det = e1e2*e1e2 - e1e1*e2e2; - if(GIM_IS_ZERO(det)) return false; - t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det; - t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det; + VEC_DIFF(p1p2, point1, point2); + GREAL p1p2e1 = VEC_DOT(p1p2, dir1); + GREAL p1p2e2 = VEC_DOT(p1p2, dir2); + det = e1e2 * e1e2 - e1e1 * e2e2; + if (GIM_IS_ZERO(det)) return false; + t1 = (e1e2 * p1p2e2 - e2e2 * p1p2e1) / det; + t2 = (e1e1 * p1p2e2 - e1e2 * p1p2e1) / det; return true; } //! Find closest points on segments -template<typename CLASS_POINT> +template <typename CLASS_POINT> SIMD_FORCE_INLINE void SEGMENT_COLLISION( - const CLASS_POINT & vA1, - const CLASS_POINT & vA2, - const CLASS_POINT & vB1, - const CLASS_POINT & vB2, - CLASS_POINT & vPointA, - CLASS_POINT & vPointB) + const CLASS_POINT &vA1, + const CLASS_POINT &vA2, + const CLASS_POINT &vB1, + const CLASS_POINT &vB2, + CLASS_POINT &vPointA, + CLASS_POINT &vPointB) { - CLASS_POINT _AD,_BD,n; - vec4f _M;//plane - VEC_DIFF(_AD,vA2,vA1); - VEC_DIFF(_BD,vB2,vB1); - VEC_CROSS(n,_AD,_BD); - GREAL _tp = VEC_DOT(n,n); - if(_tp<G_EPSILON)//ARE PARALELE - { - //project B over A - bool invert_b_order = false; - _M[0] = VEC_DOT(vB1,_AD); - _M[1] = VEC_DOT(vB2,_AD); - if(_M[0]>_M[1]) - { - invert_b_order = true; - GIM_SWAP_NUMBERS(_M[0],_M[1]); - } - _M[2] = VEC_DOT(vA1,_AD); - _M[3] = VEC_DOT(vA2,_AD); - //mid points - n[0] = (_M[0]+_M[1])*0.5f; - n[1] = (_M[2]+_M[3])*0.5f; - - if(n[0]<n[1]) - { - if(_M[1]<_M[2]) - { - vPointB = invert_b_order?vB1:vB2; - vPointA = vA1; - } - else if(_M[1]<_M[3]) - { - vPointB = invert_b_order?vB1:vB2; - CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2); - } - else - { - vPointA = vA2; - CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2); - } - } - else - { - if(_M[3]<_M[0]) - { - vPointB = invert_b_order?vB2:vB1; - vPointA = vA2; - } - else if(_M[3]<_M[1]) - { - vPointA = vA2; - CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2); - } - else - { - vPointB = invert_b_order?vB1:vB2; - CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2); - } - } - return; - } - - - VEC_CROSS(_M,n,_BD); - _M[3] = VEC_DOT(_M,vB1); - - LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1)); - /*Closest point on segment*/ - VEC_DIFF(vPointB,vPointA,vB1); - _tp = VEC_DOT(vPointB, _BD); - _tp/= VEC_DOT(_BD, _BD); - _tp = GIM_CLAMP(_tp,0.0f,1.0f); - VEC_SCALE(vPointB,_tp,_BD); - VEC_SUM(vPointB,vPointB,vB1); -} - + CLASS_POINT _AD, _BD, n; + vec4f _M; //plane + VEC_DIFF(_AD, vA2, vA1); + VEC_DIFF(_BD, vB2, vB1); + VEC_CROSS(n, _AD, _BD); + GREAL _tp = VEC_DOT(n, n); + if (_tp < G_EPSILON) //ARE PARALELE + { + //project B over A + bool invert_b_order = false; + _M[0] = VEC_DOT(vB1, _AD); + _M[1] = VEC_DOT(vB2, _AD); + if (_M[0] > _M[1]) + { + invert_b_order = true; + GIM_SWAP_NUMBERS(_M[0], _M[1]); + } + _M[2] = VEC_DOT(vA1, _AD); + _M[3] = VEC_DOT(vA2, _AD); + //mid points + n[0] = (_M[0] + _M[1]) * 0.5f; + n[1] = (_M[2] + _M[3]) * 0.5f; + + if (n[0] < n[1]) + { + if (_M[1] < _M[2]) + { + vPointB = invert_b_order ? vB1 : vB2; + vPointA = vA1; + } + else if (_M[1] < _M[3]) + { + vPointB = invert_b_order ? vB1 : vB2; + CLOSEST_POINT_ON_SEGMENT(vPointA, vPointB, vA1, vA2); + } + else + { + vPointA = vA2; + CLOSEST_POINT_ON_SEGMENT(vPointB, vPointA, vB1, vB2); + } + } + else + { + if (_M[3] < _M[0]) + { + vPointB = invert_b_order ? vB2 : vB1; + vPointA = vA2; + } + else if (_M[3] < _M[1]) + { + vPointA = vA2; + CLOSEST_POINT_ON_SEGMENT(vPointB, vPointA, vB1, vB2); + } + else + { + vPointB = invert_b_order ? vB1 : vB2; + CLOSEST_POINT_ON_SEGMENT(vPointA, vPointB, vA1, vA2); + } + } + return; + } + VEC_CROSS(_M, n, _BD); + _M[3] = VEC_DOT(_M, vB1); + LINE_PLANE_COLLISION(_M, _AD, vA1, vPointA, _tp, btScalar(0), btScalar(1)); + /*Closest point on segment*/ + VEC_DIFF(vPointB, vPointA, vB1); + _tp = VEC_DOT(vPointB, _BD); + _tp /= VEC_DOT(_BD, _BD); + _tp = GIM_CLAMP(_tp, 0.0f, 1.0f); + VEC_SCALE(vPointB, _tp, _BD); + VEC_SUM(vPointB, vPointB, vB1); +} //! Line box intersection in one dimension /*! @@ -497,37 +492,36 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION( *\param tlast the maximum projection. Assign to INFINITY at first. *\return true if there is an intersection. */ -template<typename T> -SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast) +template <typename T> +SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir, T bmin, T bmax, T &tfirst, T &tlast) { - if(GIM_IS_ZERO(dir)) + if (GIM_IS_ZERO(dir)) { - return !(pos < bmin || pos > bmax); + return !(pos < bmin || pos > bmax); } GREAL a0 = (bmin - pos) / dir; GREAL a1 = (bmax - pos) / dir; - if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1); + if (a0 > a1) GIM_SWAP_NUMBERS(a0, a1); tfirst = GIM_MAX(a0, tfirst); tlast = GIM_MIN(a1, tlast); if (tlast < tfirst) return false; return true; } - //! Sorts 3 componets -template<typename T> +template <typename T> SIMD_FORCE_INLINE void SORT_3_INDICES( - const T * values, - GUINT * order_indices) + const T *values, + GUINT *order_indices) { //get minimum order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2); //get second and third - GUINT i0 = (order_indices[0] + 1)%3; - GUINT i1 = (i0 + 1)%3; + GUINT i0 = (order_indices[0] + 1) % 3; + GUINT i1 = (i0 + 1) % 3; - if(values[i0] < values[i1]) + if (values[i0] < values[i1]) { order_indices[1] = i0; order_indices[2] = i1; @@ -539,8 +533,4 @@ SIMD_FORCE_INLINE void SORT_3_INDICES( } } - - - - -#endif // GIM_VECTOR_H_INCLUDED +#endif // GIM_VECTOR_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_bitset.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_bitset.h index 7dee48a4c7..c1fb41a5c0 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_bitset.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_bitset.h @@ -34,34 +34,32 @@ email: projectileman@yahoo.com #include "gim_array.h" - #define GUINT_BIT_COUNT 32 #define GUINT_EXPONENT 5 class gim_bitset { public: - gim_array<GUINT> m_container; - - gim_bitset() - { + gim_array<GUINT> m_container; - } + gim_bitset() + { + } - gim_bitset(GUINT bits_count) - { - resize(bits_count); - } + gim_bitset(GUINT bits_count) + { + resize(bits_count); + } - ~gim_bitset() - { - } + ~gim_bitset() + { + } inline bool resize(GUINT newsize) { GUINT oldsize = m_container.size(); - m_container.resize(newsize/GUINT_BIT_COUNT + 1,false); - while(oldsize<m_container.size()) + m_container.resize(newsize / GUINT_BIT_COUNT + 1, false); + while (oldsize < m_container.size()) { m_container[oldsize] = 0; } @@ -70,12 +68,12 @@ public: inline GUINT size() { - return m_container.size()*GUINT_BIT_COUNT; + return m_container.size() * GUINT_BIT_COUNT; } inline void set_all() { - for(GUINT i = 0;i<m_container.size();++i) + for (GUINT i = 0; i < m_container.size(); ++i) { m_container[i] = 0xffffffff; } @@ -83,7 +81,7 @@ public: inline void clear_all() { - for(GUINT i = 0;i<m_container.size();++i) + for (GUINT i = 0; i < m_container.size(); ++i) { m_container[i] = 0; } @@ -91,33 +89,29 @@ public: inline void set(GUINT bit_index) { - if(bit_index>=size()) + if (bit_index >= size()) { resize(bit_index); } - m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT-1))); + m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT - 1))); } ///Return 0 or 1 inline char get(GUINT bit_index) { - if(bit_index>=size()) + if (bit_index >= size()) { return 0; } char value = m_container[bit_index >> GUINT_EXPONENT] & - (1 << (bit_index & (GUINT_BIT_COUNT-1))); + (1 << (bit_index & (GUINT_BIT_COUNT - 1))); return value; } inline void clear(GUINT bit_index) { - m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT-1))); + m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT - 1))); } }; - - - - -#endif // GIM_CONTAINERS_H_INCLUDED +#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_box_collision.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_box_collision.h index a051b4fdbf..9f7cbe732f 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_box_collision.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_box_collision.h @@ -35,8 +35,6 @@ email: projectileman@yahoo.com #include "gim_basic_geometry_operations.h" #include "LinearMath/btTransform.h" - - //SIMD_FORCE_INLINE bool test_cross_edge_box( // const btVector3 & edge, // const btVector3 & absolute_edge, @@ -99,52 +97,50 @@ email: projectileman@yahoo.com #ifndef TEST_CROSS_EDGE_BOX_MCR -#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)\ - {\ - GIM_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) \ + { \ + GIM_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; \ + } #endif -#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_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_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_Z_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \ + { \ + TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 1, 0, 0, 1); \ + } //! Class for transforming a model1 to the space of model0 class GIM_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); + 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(); @@ -154,42 +150,40 @@ public: { } - - GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0) + GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0) { - COPY_MATRIX_3X3(m_R1to0,trans1_to_0) - MAT_GET_TRANSLATION(trans1_to_0,m_T1to0) + COPY_MATRIX_3X3(m_R1to0, trans1_to_0) + MAT_GET_TRANSLATION(trans1_to_0, m_T1to0) calc_absolute_matrix(); } //! 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) { - m_R1to0 = trans0.getBasis().transpose(); 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(); } //! 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) + SIMD_FORCE_INLINE btVector3 transform(const btVector3 &point) { - 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; } }; @@ -205,34 +199,34 @@ public: btVector3 m_max; GIM_AABB() - {} - + { + } - GIM_AABB(const btVector3 & V1, - const btVector3 & V2, - const btVector3 & V3) + GIM_AABB(const btVector3 &V1, + const btVector3 &V2, + const btVector3 &V3) { - m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]); } - GIM_AABB(const btVector3 & V1, - const btVector3 & V2, - const btVector3 & V3, + GIM_AABB(const btVector3 &V1, + const btVector3 &V2, + const btVector3 &V3, GREAL margin) { - m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]); m_min[0] -= margin; m_min[1] -= margin; @@ -242,13 +236,11 @@ public: m_max[2] += margin; } - GIM_AABB(const GIM_AABB &other): - m_min(other.m_min),m_max(other.m_max) + GIM_AABB(const GIM_AABB &other) : m_min(other.m_min), m_max(other.m_max) { } - GIM_AABB(const GIM_AABB &other,btScalar margin ): - m_min(other.m_min),m_max(other.m_max) + GIM_AABB(const GIM_AABB &other, btScalar margin) : m_min(other.m_min), m_max(other.m_max) { m_min[0] -= margin; m_min[1] -= margin; @@ -289,34 +281,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] = GIM_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = GIM_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] = GIM_MIN3(V1[0],V2[0],V3[0]); - m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]); - m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]); + m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]); + m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]); + m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]); - m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]); - m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]); - m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]); + m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]); + m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]); + m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]); m_min[0] -= margin; m_min[1] -= margin; @@ -327,74 +319,73 @@ 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; } //! Merges a Box - SIMD_FORCE_INLINE void merge(const GIM_AABB & box) + SIMD_FORCE_INLINE void merge(const GIM_AABB &box) { - m_min[0] = GIM_MIN(m_min[0],box.m_min[0]); - m_min[1] = GIM_MIN(m_min[1],box.m_min[1]); - m_min[2] = GIM_MIN(m_min[2],box.m_min[2]); + m_min[0] = GIM_MIN(m_min[0], box.m_min[0]); + m_min[1] = GIM_MIN(m_min[1], box.m_min[1]); + m_min[2] = GIM_MIN(m_min[2], box.m_min[2]); - m_max[0] = GIM_MAX(m_max[0],box.m_max[0]); - m_max[1] = GIM_MAX(m_max[1],box.m_max[1]); - m_max[2] = GIM_MAX(m_max[2],box.m_max[2]); + m_max[0] = GIM_MAX(m_max[0], box.m_max[0]); + m_max[1] = GIM_MAX(m_max[1], box.m_max[1]); + m_max[2] = GIM_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] = GIM_MIN(m_min[0],point[0]); - m_min[1] = GIM_MIN(m_min[1],point[1]); - m_min[2] = GIM_MIN(m_min[2],point[2]); + m_min[0] = GIM_MIN(m_min[0], point[0]); + m_min[1] = GIM_MIN(m_min[1], point[1]); + m_min[2] = GIM_MIN(m_min[2], point[2]); - m_max[0] = GIM_MAX(m_max[0],point[0]); - m_max[1] = GIM_MAX(m_max[1],point[1]); - m_max[2] = GIM_MAX(m_max[2],point[2]); + m_max[0] = GIM_MAX(m_max[0], point[0]); + m_max[1] = GIM_MAX(m_max[1], point[1]); + m_max[2] = GIM_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 ¢er, 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 GIM_AABB & other, GIM_AABB & intersection) const + SIMD_FORCE_INLINE void find_intersection(const GIM_AABB &other, GIM_AABB &intersection) const { - intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]); - intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]); - intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]); + intersection.m_min[0] = GIM_MAX(other.m_min[0], m_min[0]); + intersection.m_min[1] = GIM_MAX(other.m_min[1], m_min[1]); + intersection.m_min[2] = GIM_MAX(other.m_min[2], m_min[2]); - intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]); - intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]); - intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]); + intersection.m_max[0] = GIM_MIN(other.m_max[0], m_max[0]); + intersection.m_max[1] = GIM_MIN(other.m_max[1], m_max[1]); + intersection.m_max[2] = GIM_MIN(other.m_max[2], m_max[2]); } - - SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const + SIMD_FORCE_INLINE bool has_collision(const GIM_AABB &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; } @@ -406,35 +397,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) + SIMD_FORCE_INLINE bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) { - 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(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false; + if (GIM_GREATER(Dx, extents[0]) && Dx * vdir[0] >= 0.0f) return false; btScalar Dy = vorigin[1] - center[1]; - if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false; + if (GIM_GREATER(Dy, extents[1]) && Dy * vdir[1] >= 0.0f) return false; btScalar Dz = vorigin[2] - center[2]; - if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false; - + if (GIM_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; @@ -442,22 +432,22 @@ public: SIMD_FORCE_INLINE ePLANE_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 G_BACK_PLANE; // 0 + return G_BACK_PLANE; // 0 } - if(plane[3]+BOX_PLANE_EPSILON >=_fmin) + if (plane[3] + BOX_PLANE_EPSILON >= _fmin) { - return G_COLLIDE_PLANE; //1 + return G_COLLIDE_PLANE; //1 } - return G_FRONT_PLANE;//2 + return G_FRONT_PLANE; //2 } - SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0) + SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB &box, btTransform &trans1_to_0) { GIM_AABB tbox = box; tbox.appy_transform(trans1_to_0); @@ -466,52 +456,50 @@ public: //! transcache is the transformation cache from box to this AABB SIMD_FORCE_INLINE bool overlapping_trans_cache( - const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) + const GIM_AABB &box, const GIM_BOX_BOX_TRANSFORM_CACHE &transcache, bool fulltest) { - //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(GIM_GREATER(T[i], t)) return false; + if (GIM_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 = MAT_DOT_COL(transcache.m_R1to0,T,i); - t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i]; - if(GIM_GREATER(t,t2)) return false; + t = MAT_DOT_COL(transcache.m_R1to0, T, i); + t2 = MAT_DOT_COL(transcache.m_AR, ea, i) + eb[i]; + if (GIM_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(GIM_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 (GIM_GREATER(t, t2)) return false; } } } @@ -520,7 +508,7 @@ public: //! Simple test for planes. SIMD_FORCE_INLINE bool collide_plane( - const btVector4 & plane) + const btVector4 &plane) { ePLANE_INTERSECTION_TYPE classify = plane_classify(plane); return (classify == G_COLLIDE_PLANE); @@ -528,15 +516,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 btVector3 &p1, + const btVector3 &p2, + const btVector3 &p3, + const btVector4 &triangle_plane) { - 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); @@ -546,30 +534,29 @@ 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; } @@ -577,17 +564,15 @@ public: #ifndef BT_BOX_COLLISION_H_INCLUDED //! 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 - - -#endif // GIM_BOX_COLLISION_H_INCLUDED +#endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.cpp b/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.cpp index 0c3d7ba8db..0c7a6b7fc1 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.cpp @@ -28,67 +28,64 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_box_set.h" - GUINT GIM_BOX_TREE::_calc_splitting_axis( - gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex) + gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex) { GUINT i; - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); - GUINT numIndices = endIndex-startIndex; + btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.)); + btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.)); + GUINT numIndices = endIndex - startIndex; - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - means+=center; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + means += center; } - means *= (btScalar(1.)/(btScalar)numIndices); + means *= (btScalar(1.) / (btScalar)numIndices); - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max + - primitive_boxes[i].m_bound.m_min); - btVector3 diff2 = center-means; + btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max + + primitive_boxes[i].m_bound.m_min); + btVector3 diff2 = center - means; diff2 = diff2 * diff2; variance += diff2; } - variance *= (btScalar(1.)/ ((btScalar)numIndices-1) ); + variance *= (btScalar(1.) / ((btScalar)numIndices - 1)); return variance.maxAxis(); } - GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index( - gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, + gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex, GUINT splitAxis) { GUINT i; - GUINT splitIndex =startIndex; + GUINT splitIndex = startIndex; GUINT numIndices = endIndex - startIndex; // average of centers btScalar splitValue = 0.0f; - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - splitValue+= 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] + - primitive_boxes[i].m_bound.m_min[splitAxis]); + splitValue += 0.5f * (primitive_boxes[i].m_bound.m_max[splitAxis] + + primitive_boxes[i].m_bound.m_min[splitAxis]); } splitValue /= (btScalar)numIndices; //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. - for (i=startIndex;i<endIndex;i++) + for (i = startIndex; i < endIndex; i++) { - btScalar center = 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] + - primitive_boxes[i].m_bound.m_min[splitAxis]); + btScalar center = 0.5f * (primitive_boxes[i].m_bound.m_max[splitAxis] + + primitive_boxes[i].m_bound.m_min[splitAxis]); if (center > splitValue) { //swap - primitive_boxes.swap(i,splitIndex); + primitive_boxes.swap(i, splitIndex); splitIndex++; } } @@ -102,28 +99,27 @@ GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index( //bool unbalanced2 = true; //this should be safe too: - GUINT rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + GUINT rangeBalancedIndices = numIndices / 3; + bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); if (unbalanced) { - splitIndex = startIndex+ (numIndices>>1); + splitIndex = startIndex + (numIndices >> 1); } - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); + btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex)))); return splitIndex; } - -void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex) +void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex) { GUINT current_index = m_num_nodes++; - btAssert((endIndex-startIndex)>0); + btAssert((endIndex - startIndex) > 0); - if((endIndex-startIndex) == 1) //we got a leaf - { + if ((endIndex - startIndex) == 1) //we got a leaf + { m_node_array[current_index].m_left = 0; m_node_array[current_index].m_right = 0; m_node_array[current_index].m_escapeIndex = 0; @@ -138,8 +134,8 @@ void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, G GUINT splitIndex; //calc this node bounding box - m_node_array[current_index].m_bound.invalidate(); - for (splitIndex=startIndex;splitIndex<endIndex;splitIndex++) + m_node_array[current_index].m_bound.invalidate(); + for (splitIndex = startIndex; splitIndex < endIndex; splitIndex++) { m_node_array[current_index].m_bound.merge(primitive_boxes[splitIndex].m_bound); } @@ -147,36 +143,34 @@ void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, G //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. //split axis - splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); + splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex); splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex,splitIndex); + primitive_boxes, startIndex, endIndex, splitIndex); //configure this inner node : the left node index m_node_array[current_index].m_left = m_num_nodes; //build left child tree - _build_sub_tree(primitive_boxes, startIndex, splitIndex ); + _build_sub_tree(primitive_boxes, startIndex, splitIndex); //configure this inner node : the right node index m_node_array[current_index].m_right = m_num_nodes; //build right child tree - _build_sub_tree(primitive_boxes, splitIndex ,endIndex); + _build_sub_tree(primitive_boxes, splitIndex, endIndex); //configure this inner node : the escape index - m_node_array[current_index].m_escapeIndex = m_num_nodes - current_index; + m_node_array[current_index].m_escapeIndex = m_num_nodes - current_index; } //! stackless build tree void GIM_BOX_TREE::build_tree( - gim_array<GIM_AABB_DATA> & primitive_boxes) + gim_array<GIM_AABB_DATA>& primitive_boxes) { // initialize node count to 0 m_num_nodes = 0; // allocate nodes - m_node_array.resize(primitive_boxes.size()*2); - + m_node_array.resize(primitive_boxes.size() * 2); + _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); } - - diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.h index 61d190a7df..0522007e4f 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_box_set.h @@ -33,54 +33,51 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_array.h" #include "gim_radixsort.h" #include "gim_box_collision.h" #include "gim_tri_collision.h" - - //! Overlapping pair struct GIM_PAIR { - GUINT m_index1; - GUINT m_index2; - GIM_PAIR() - {} + GUINT m_index1; + GUINT m_index2; + GIM_PAIR() + { + } - GIM_PAIR(const GIM_PAIR & p) - { - m_index1 = p.m_index1; - m_index2 = p.m_index2; + GIM_PAIR(const GIM_PAIR& p) + { + m_index1 = p.m_index1; + m_index2 = p.m_index2; } GIM_PAIR(GUINT index1, GUINT index2) - { - m_index1 = index1; - m_index2 = index2; + { + m_index1 = index1; + m_index2 = index2; } }; //! A pairset array -class gim_pair_set: public gim_array<GIM_PAIR> +class gim_pair_set : public gim_array<GIM_PAIR> { public: - gim_pair_set():gim_array<GIM_PAIR>(32) + gim_pair_set() : gim_array<GIM_PAIR>(32) { } - inline void push_pair(GUINT index1,GUINT index2) + inline void push_pair(GUINT index1, GUINT index2) { - push_back(GIM_PAIR(index1,index2)); + push_back(GIM_PAIR(index1, index2)); } - inline void push_pair_inv(GUINT index1,GUINT index2) + inline void push_pair_inv(GUINT index1, GUINT index2) { - push_back(GIM_PAIR(index2,index1)); + push_back(GIM_PAIR(index2, index1)); } }; - //! Prototype Base class for primitive classification /*! This class is a wrapper for primitive collections. @@ -90,16 +87,14 @@ This class can manage Compound shapes and trimeshes, and if it is managing trime class GIM_PRIMITIVE_MANAGER_PROTOTYPE { public: - virtual ~GIM_PRIMITIVE_MANAGER_PROTOTYPE() {} //! determines if this manager consist on only triangles, which special case will be optimized virtual bool is_trimesh() = 0; virtual GUINT get_primitive_count() = 0; - virtual void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) = 0; - virtual void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) = 0; + virtual void get_primitive_box(GUINT prim_index, GIM_AABB& primbox) = 0; + virtual void get_primitive_triangle(GUINT prim_index, GIM_TRIANGLE& triangle) = 0; }; - struct GIM_AABB_DATA { GIM_AABB m_bound; @@ -110,22 +105,22 @@ struct GIM_AABB_DATA struct GIM_BOX_TREE_NODE { GIM_AABB m_bound; - GUINT m_left;//!< Left subtree - GUINT m_right;//!< Right subtree - GUINT m_escapeIndex;//!< Scape index for traversing - GUINT m_data;//!< primitive index if apply + GUINT m_left; //!< Left subtree + GUINT m_right; //!< Right subtree + GUINT m_escapeIndex; //!< Scape index for traversing + GUINT m_data; //!< primitive index if apply GIM_BOX_TREE_NODE() { - m_left = 0; - m_right = 0; - m_escapeIndex = 0; - m_data = 0; + m_left = 0; + m_right = 0; + m_escapeIndex = 0; + m_data = 0; } SIMD_FORCE_INLINE bool is_leaf_node() const { - return (!m_left && !m_right); + return (!m_left && !m_right); } }; @@ -135,14 +130,16 @@ class GIM_BOX_TREE protected: GUINT m_num_nodes; gim_array<GIM_BOX_TREE_NODE> m_node_array; + protected: GUINT _sort_and_calc_splitting_index( - gim_array<GIM_AABB_DATA> & primitive_boxes, - GUINT startIndex, GUINT endIndex, GUINT splitAxis); + gim_array<GIM_AABB_DATA>& primitive_boxes, + GUINT startIndex, GUINT endIndex, GUINT splitAxis); - GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex); + GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex); + + void _build_sub_tree(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex); - void _build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex); public: GIM_BOX_TREE() { @@ -151,7 +148,7 @@ public: //! prototype functions for box tree management //!@{ - void build_tree(gim_array<GIM_AABB_DATA> & primitive_boxes); + void build_tree(gim_array<GIM_AABB_DATA>& primitive_boxes); SIMD_FORCE_INLINE void clearNodes() { @@ -176,22 +173,22 @@ public: return m_node_array[nodeindex].m_data; } - SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const { bound = m_node_array[nodeindex].m_bound; } - SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound) + SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound) { m_node_array[nodeindex].m_bound = bound; } - SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const + SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const { return m_node_array[nodeindex].m_left; } - SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const + SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const { return m_node_array[nodeindex].m_right; } @@ -204,78 +201,78 @@ public: //!@} }; - //! Generic Box Tree Template /*! This class offers an structure for managing a box tree of primitives. Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and a Box tree structure ( like GIM_BOX_TREE). */ -template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE> +template <typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE> class GIM_BOX_TREE_TEMPLATE_SET { protected: _GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager; _GIM_BOX_TREE_PROTOTYPE m_box_tree; + protected: //stackless refit SIMD_FORCE_INLINE void refit() { GUINT nodecount = getNodeCount(); - while(nodecount--) + while (nodecount--) { - if(isLeafNode(nodecount)) + if (isLeafNode(nodecount)) { GIM_AABB leafbox; - m_primitive_manager.get_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); + m_primitive_manager.get_primitive_box(getNodeData(nodecount), leafbox); + setNodeBound(nodecount, leafbox); } else { //get left bound GUINT childindex = getLeftNodeIndex(nodecount); GIM_AABB bound; - getNodeBound(childindex,bound); + getNodeBound(childindex, bound); //get right bound childindex = getRightNodeIndex(nodecount); GIM_AABB bound2; - getNodeBound(childindex,bound2); + getNodeBound(childindex, bound2); bound.merge(bound2); - setNodeBound(nodecount,bound); + setNodeBound(nodecount, bound); } } } -public: +public: GIM_BOX_TREE_TEMPLATE_SET() { } - SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const + SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const { GIM_AABB totalbox; getNodeBound(0, totalbox); return totalbox; } - SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & primitive_manager) + SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& primitive_manager) { m_primitive_manager = primitive_manager; } - const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const + const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() const { return m_primitive_manager; } - _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() + _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() { return m_primitive_manager; } -//! node manager prototype functions -///@{ + //! node manager prototype functions + ///@{ //! this attemps to refit the box set. SIMD_FORCE_INLINE void update() @@ -288,19 +285,19 @@ public: { //obtain primitive boxes gim_array<GIM_AABB_DATA> primitive_boxes; - primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false); + primitive_boxes.resize(m_primitive_manager.get_primitive_count(), false); - for (GUINT i = 0;i<primitive_boxes.size() ;i++ ) + for (GUINT i = 0; i < primitive_boxes.size(); i++) { - m_primitive_manager.get_primitive_box(i,primitive_boxes[i].m_bound); - primitive_boxes[i].m_data = i; + m_primitive_manager.get_primitive_box(i, primitive_boxes[i].m_bound); + primitive_boxes[i].m_data = i; } m_box_tree.build_tree(primitive_boxes); } //! returns the indices of the primitives in the m_primitive_manager - SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB & box, gim_array<GUINT> & collided_results) const + SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB& box, gim_array<GUINT>& collided_results) const { GUINT curIndex = 0; GUINT numNodes = getNodeCount(); @@ -308,7 +305,7 @@ public: while (curIndex < numNodes) { GIM_AABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data @@ -328,26 +325,26 @@ public: else { //skip node - curIndex+= getScapeNodeIndex(curIndex); + curIndex += getScapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } //! returns the indices of the primitives in the m_primitive_manager - SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB & box, - const btTransform & transform, gim_array<GUINT> & collided_results) const + SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB& box, + const btTransform& transform, gim_array<GUINT>& collided_results) const { - GIM_AABB transbox=box; + GIM_AABB transbox = box; transbox.appy_transform(transform); - return boxQuery(transbox,collided_results); + return boxQuery(transbox, collided_results); } //! returns the indices of the primitives in the m_primitive_manager SIMD_FORCE_INLINE bool rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - gim_array<GUINT> & collided_results) const + const btVector3& ray_dir, const btVector3& ray_origin, + gim_array<GUINT>& collided_results) const { GUINT curIndex = 0; GUINT numNodes = getNodeCount(); @@ -355,16 +352,16 @@ public: while (curIndex < numNodes) { GIM_AABB bound; - getNodeBound(curIndex,bound); + getNodeBound(curIndex, bound); //catch bugs in tree data - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir); bool isleafnode = isLeafNode(curIndex); if (isleafnode && aabbOverlap) { - collided_results.push_back(getNodeData( curIndex)); + collided_results.push_back(getNodeData(curIndex)); } if (aabbOverlap || isleafnode) @@ -375,10 +372,10 @@ public: else { //skip node - curIndex+= getScapeNodeIndex(curIndex); + curIndex += getScapeNodeIndex(curIndex); } } - if(collided_results.size()>0) return true; + if (collided_results.size() > 0) return true; return false; } @@ -389,7 +386,7 @@ public: } //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const + SIMD_FORCE_INLINE bool isTrimesh() const { return m_primitive_manager.is_trimesh(); } @@ -411,12 +408,12 @@ public: return m_box_tree.getNodeData(nodeindex); } - SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const + SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const { m_box_tree.getNodeBound(nodeindex, bound); } - SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound) + SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound) { m_box_tree.setNodeBound(nodeindex, bound); } @@ -436,36 +433,30 @@ public: return m_box_tree.getScapeNodeIndex(nodeindex); } - SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const + SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex, GIM_TRIANGLE& triangle) const { - m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle); + m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex), triangle); } - }; //! Class for Box Tree Sets /*! this has the GIM_BOX_TREE implementation for bounding boxes. */ -template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE> -class GIM_BOX_TREE_SET: public GIM_BOX_TREE_TEMPLATE_SET< _GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE> +template <typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE> +class GIM_BOX_TREE_SET : public GIM_BOX_TREE_TEMPLATE_SET<_GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE> { public: - }; - - - - /// GIM_BOX_SET collision methods -template<typename BOX_SET_CLASS0,typename BOX_SET_CLASS1> +template <typename BOX_SET_CLASS0, typename BOX_SET_CLASS1> class GIM_TREE_TREE_COLLIDER { public: - gim_pair_set * m_collision_pairs; - BOX_SET_CLASS0 * m_boxset0; - BOX_SET_CLASS1 * m_boxset1; + gim_pair_set* m_collision_pairs; + BOX_SET_CLASS0* m_boxset0; + BOX_SET_CLASS1* m_boxset1; GUINT current_node0; GUINT current_node1; bool node0_is_leaf; @@ -483,18 +474,18 @@ public: GIM_TRIANGLE m_tri1; btVector4 m_tri1_plane; - public: GIM_TREE_TREE_COLLIDER() { current_node0 = G_UINT_INFINITY; current_node1 = G_UINT_INFINITY; } + protected: SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0) { - if(node0_has_triangle) return; - m_boxset0->getNodeTriangle(node0,m_tri0); + if (node0_has_triangle) return; + m_boxset0->getNodeTriangle(node0, m_tri0); //transform triangle m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]); m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]); @@ -506,8 +497,8 @@ protected: SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1) { - if(node1_has_triangle) return; - m_boxset1->getNodeTriangle(node1,m_tri1); + if (node1_has_triangle) return; + m_boxset1->getNodeTriangle(node1, m_tri1); //transform triangle m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]); m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]); @@ -519,8 +510,8 @@ protected: SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0) { - if(node0 == current_node0) return; - m_boxset0->getNodeBound(node0,m_box0); + if (node0 == current_node0) return; + m_boxset0->getNodeBound(node0, m_box0); node0_is_leaf = m_boxset0->isLeafNode(node0); node0_has_triangle = false; current_node0 = node0; @@ -528,21 +519,21 @@ protected: SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1) { - if(node1 == current_node1) return; - m_boxset1->getNodeBound(node1,m_box1); + if (node1 == current_node1) return; + m_boxset1->getNodeBound(node1, m_box1); node1_is_leaf = m_boxset1->isLeafNode(node1); node1_has_triangle = false; current_node1 = node1; } - SIMD_FORCE_INLINE bool node_collision(GUINT node0 ,GUINT node1) + SIMD_FORCE_INLINE bool node_collision(GUINT node0, GUINT node1) { retrieve_node0_info(node0); retrieve_node1_info(node1); - bool result = m_box0.overlapping_trans_cache(m_box1,trans_cache_1to0,true); - if(!result) return false; + bool result = m_box0.overlapping_trans_cache(m_box1, trans_cache_1to0, true); + if (!result) return false; - if(t0_is_trimesh && node0_is_leaf) + if (t0_is_trimesh && node0_is_leaf) { //perform primitive vs box collision retrieve_node0_triangle(node0); @@ -550,14 +541,14 @@ protected: m_box1.increment_margin(m_tri0.m_margin); result = m_box1.collide_triangle_exact( - m_tri0.m_vertices[0],m_tri0.m_vertices[1],m_tri0.m_vertices[2],m_tri0_plane); + m_tri0.m_vertices[0], m_tri0.m_vertices[1], m_tri0.m_vertices[2], m_tri0_plane); m_box1.increment_margin(-m_tri0.m_margin); - if(!result) return false; + if (!result) return false; return true; } - else if(t1_is_trimesh && node1_is_leaf) + else if (t1_is_trimesh && node1_is_leaf) { //perform primitive vs box collision retrieve_node1_triangle(node1); @@ -565,11 +556,11 @@ protected: m_box0.increment_margin(m_tri1.m_margin); result = m_box0.collide_triangle_exact( - m_tri1.m_vertices[0],m_tri1.m_vertices[1],m_tri1.m_vertices[2],m_tri1_plane); + m_tri1.m_vertices[0], m_tri1.m_vertices[1], m_tri1.m_vertices[2], m_tri1_plane); m_box0.increment_margin(-m_tri1.m_margin); - if(!result) return false; + if (!result) return false; return true; } return true; @@ -582,40 +573,39 @@ protected: stack_collisions.reserve(32); //add the first pair - stack_collisions.push_pair(0,0); + stack_collisions.push_pair(0, 0); - - while(stack_collisions.size()) + while (stack_collisions.size()) { //retrieve the last pair and pop GUINT node0 = stack_collisions.back().m_index1; GUINT node1 = stack_collisions.back().m_index2; stack_collisions.pop_back(); - if(node_collision(node0,node1)) // a collision is found + if (node_collision(node0, node1)) // a collision is found { - if(node0_is_leaf) + if (node0_is_leaf) { - if(node1_is_leaf) + if (node1_is_leaf) { - m_collision_pairs->push_pair(m_boxset0->getNodeData(node0),m_boxset1->getNodeData(node1)); + m_collision_pairs->push_pair(m_boxset0->getNodeData(node0), m_boxset1->getNodeData(node1)); } else { //collide left - stack_collisions.push_pair(node0,m_boxset1->getLeftNodeIndex(node1)); + stack_collisions.push_pair(node0, m_boxset1->getLeftNodeIndex(node1)); //collide right - stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1)); + stack_collisions.push_pair(node0, m_boxset1->getRightNodeIndex(node1)); } } else { - if(node1_is_leaf) + if (node1_is_leaf) { //collide left - stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1); + stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0), node1); //collide right - stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1); + stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0), node1); } else { @@ -624,36 +614,36 @@ protected: GUINT left1 = m_boxset1->getLeftNodeIndex(node1); GUINT right1 = m_boxset1->getRightNodeIndex(node1); //collide left - stack_collisions.push_pair(left0,left1); + stack_collisions.push_pair(left0, left1); //collide right - stack_collisions.push_pair(left0,right1); + stack_collisions.push_pair(left0, right1); //collide left - stack_collisions.push_pair(right0,left1); + stack_collisions.push_pair(right0, left1); //collide right - stack_collisions.push_pair(right0,right1); + stack_collisions.push_pair(right0, right1); - }// else if node1 is not a leaf - }// else if node0 is not a leaf + } // else if node1 is not a leaf + } // else if node0 is not a leaf - }// if(node_collision(node0,node1)) - }//while(stack_collisions.size()) + } // if(node_collision(node0,node1)) + } //while(stack_collisions.size()) } + public: - void find_collision(BOX_SET_CLASS0 * boxset1, const btTransform & trans1, - BOX_SET_CLASS1 * boxset2, const btTransform & trans2, - gim_pair_set & collision_pairs, bool complete_primitive_tests = true) + void find_collision(BOX_SET_CLASS0* boxset1, const btTransform& trans1, + BOX_SET_CLASS1* boxset2, const btTransform& trans2, + gim_pair_set& collision_pairs, bool complete_primitive_tests = true) { m_collision_pairs = &collision_pairs; m_boxset0 = boxset1; m_boxset1 = boxset2; - trans_cache_1to0.calc_from_homogenic(trans1,trans2); + trans_cache_1to0.calc_from_homogenic(trans1, trans2); - trans_cache_0to1 = trans2.inverse(); + trans_cache_0to1 = trans2.inverse(); trans_cache_0to1 *= trans1; - - if(complete_primitive_tests) + if (complete_primitive_tests) { t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh(); t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh(); @@ -668,7 +658,4 @@ public: } }; - -#endif // GIM_BOXPRUNING_H_INCLUDED - - +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_clip_polygon.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_clip_polygon.h index e342459ce5..57b9c5c91a 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_clip_polygon.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_clip_polygon.h @@ -33,91 +33,86 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - //! This function calcs the distance from a 3D plane class DISTANCE_PLANE_3D_FUNC { public: - template<typename CLASS_POINT,typename CLASS_PLANE> - inline GREAL operator()(const CLASS_PLANE & plane, const CLASS_POINT & point) + template <typename CLASS_POINT, typename CLASS_PLANE> + inline GREAL operator()(const CLASS_PLANE& plane, const CLASS_POINT& point) { return DISTANCE_PLANE_POINT(plane, point); } }; - - -template<typename CLASS_POINT> +template <typename CLASS_POINT> SIMD_FORCE_INLINE void PLANE_CLIP_POLYGON_COLLECT( - const CLASS_POINT & point0, - const CLASS_POINT & point1, - GREAL dist0, - GREAL dist1, - CLASS_POINT * clipped, - GUINT & clipped_count) + const CLASS_POINT& point0, + const CLASS_POINT& point1, + GREAL dist0, + GREAL dist1, + CLASS_POINT* clipped, + GUINT& clipped_count) { - GUINT _prevclassif = (dist0>G_EPSILON); - GUINT _classif = (dist1>G_EPSILON); - if(_classif!=_prevclassif) + GUINT _prevclassif = (dist0 > G_EPSILON); + GUINT _classif = (dist1 > G_EPSILON); + if (_classif != _prevclassif) { - GREAL blendfactor = -dist0/(dist1-dist0); - VEC_BLEND(clipped[clipped_count],point0,point1,blendfactor); + GREAL blendfactor = -dist0 / (dist1 - dist0); + VEC_BLEND(clipped[clipped_count], point0, point1, blendfactor); clipped_count++; } - if(!_classif) + if (!_classif) { - VEC_COPY(clipped[clipped_count],point1); + VEC_COPY(clipped[clipped_count], point1); clipped_count++; } } - //! Clips a polygon by a plane /*! *\return The count of the clipped counts */ -template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC> +template <typename CLASS_POINT, typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC> SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC( - const CLASS_PLANE & plane, - const CLASS_POINT * polygon_points, - GUINT polygon_point_count, - CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func) + const CLASS_PLANE& plane, + const CLASS_POINT* polygon_points, + GUINT polygon_point_count, + CLASS_POINT* clipped, DISTANCE_PLANE_FUNC distance_func) { - GUINT clipped_count = 0; + GUINT clipped_count = 0; - - //clip first point - GREAL firstdist = distance_func(plane,polygon_points[0]);; - if(!(firstdist>G_EPSILON)) + //clip first point + GREAL firstdist = distance_func(plane, polygon_points[0]); + ; + if (!(firstdist > G_EPSILON)) { - VEC_COPY(clipped[clipped_count],polygon_points[0]); + VEC_COPY(clipped[clipped_count], polygon_points[0]); clipped_count++; } GREAL olddist = firstdist; - for(GUINT _i=1;_i<polygon_point_count;_i++) - { - GREAL dist = distance_func(plane,polygon_points[_i]); + for (GUINT _i = 1; _i < polygon_point_count; _i++) + { + GREAL dist = distance_func(plane, polygon_points[_i]); PLANE_CLIP_POLYGON_COLLECT( - polygon_points[_i-1],polygon_points[_i], - olddist, - dist, - clipped, - clipped_count); + polygon_points[_i - 1], polygon_points[_i], + olddist, + dist, + clipped, + clipped_count); - - olddist = dist; + olddist = dist; } - //RETURN TO FIRST point + //RETURN TO FIRST point PLANE_CLIP_POLYGON_COLLECT( - polygon_points[polygon_point_count-1],polygon_points[0], - olddist, - firstdist, - clipped, - clipped_count); + polygon_points[polygon_point_count - 1], polygon_points[0], + olddist, + firstdist, + clipped, + clipped_count); return clipped_count; } @@ -126,85 +121,79 @@ SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC( /*! *\return The count of the clipped counts */ -template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC> +template <typename CLASS_POINT, typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC> SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE_GENERIC( - const CLASS_PLANE & plane, - const CLASS_POINT & point0, - const CLASS_POINT & point1, - const CLASS_POINT & point2, - CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func) + const CLASS_PLANE& plane, + const CLASS_POINT& point0, + const CLASS_POINT& point1, + const CLASS_POINT& point2, + CLASS_POINT* clipped, DISTANCE_PLANE_FUNC distance_func) { - GUINT clipped_count = 0; + GUINT clipped_count = 0; - //clip first point - GREAL firstdist = distance_func(plane,point0);; - if(!(firstdist>G_EPSILON)) + //clip first point + GREAL firstdist = distance_func(plane, point0); + ; + if (!(firstdist > G_EPSILON)) { - VEC_COPY(clipped[clipped_count],point0); + VEC_COPY(clipped[clipped_count], point0); clipped_count++; } // point 1 GREAL olddist = firstdist; - GREAL dist = distance_func(plane,point1); + GREAL dist = distance_func(plane, point1); PLANE_CLIP_POLYGON_COLLECT( - point0,point1, - olddist, - dist, - clipped, - clipped_count); + point0, point1, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - // point 2 - dist = distance_func(plane,point2); + dist = distance_func(plane, point2); PLANE_CLIP_POLYGON_COLLECT( - point1,point2, - olddist, - dist, - clipped, - clipped_count); + point1, point2, + olddist, + dist, + clipped, + clipped_count); olddist = dist; - - //RETURN TO FIRST point PLANE_CLIP_POLYGON_COLLECT( - point2,point0, - olddist, - firstdist, - clipped, - clipped_count); + point2, point0, + olddist, + firstdist, + clipped, + clipped_count); return clipped_count; } - -template<typename CLASS_POINT,typename CLASS_PLANE> +template <typename CLASS_POINT, typename CLASS_PLANE> SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON3D( - const CLASS_PLANE & plane, - const CLASS_POINT * polygon_points, - GUINT polygon_point_count, - CLASS_POINT * clipped) + const CLASS_PLANE& plane, + const CLASS_POINT* polygon_points, + GUINT polygon_point_count, + CLASS_POINT* clipped) { - return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,polygon_points,polygon_point_count,clipped,DISTANCE_PLANE_3D_FUNC()); + return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT, CLASS_PLANE>(plane, polygon_points, polygon_point_count, clipped, DISTANCE_PLANE_3D_FUNC()); } - -template<typename CLASS_POINT,typename CLASS_PLANE> +template <typename CLASS_POINT, typename CLASS_PLANE> SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D( - const CLASS_PLANE & plane, - const CLASS_POINT & point0, - const CLASS_POINT & point1, - const CLASS_POINT & point2, - CLASS_POINT * clipped) + const CLASS_PLANE& plane, + const CLASS_POINT& point0, + const CLASS_POINT& point1, + const CLASS_POINT& point2, + CLASS_POINT* clipped) { - return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,point0,point1,point2,clipped,DISTANCE_PLANE_3D_FUNC()); + return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT, CLASS_PLANE>(plane, point0, point1, point2, clipped, DISTANCE_PLANE_3D_FUNC()); } - - -#endif // GIM_TRI_COLLISION_H_INCLUDED +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_contact.cpp b/thirdparty/bullet/BulletCollision/Gimpact/gim_contact.cpp index 20e41de089..390225709e 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_contact.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_contact.cpp @@ -33,91 +33,89 @@ email: projectileman@yahoo.com #define MAX_COINCIDENT 8 void gim_contact_array::merge_contacts( - const gim_contact_array & contacts, bool normal_contact_average) + const gim_contact_array& contacts, bool normal_contact_average) { clear(); - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts.back()); return; } gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size()); - keycontacts.resize(contacts.size(),false); + keycontacts.resize(contacts.size(), false); //fill key contacts GUINT i; - for (i = 0;i<contacts.size() ;i++ ) + for (i = 0; i < contacts.size(); i++) { keycontacts[i].m_key = contacts[i].calc_key_contact(); keycontacts[i].m_value = i; } //sort keys - gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR()); + gim_heap_sort(keycontacts.pointer(), keycontacts.size(), GIM_RSORT_TOKEN_COMPARATOR()); // Merge contacts - GUINT coincident_count=0; + GUINT coincident_count = 0; btVector3 coincident_normals[MAX_COINCIDENT]; GUINT last_key = keycontacts[0].m_key; GUINT key = 0; push_back(contacts[keycontacts[0].m_value]); - GIM_CONTACT * pcontact = &back(); + GIM_CONTACT* pcontact = &back(); - - - for( i=1;i<keycontacts.size();i++) + for (i = 1; i < keycontacts.size(); i++) { - key = keycontacts[i].m_key; - const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value]; + key = keycontacts[i].m_key; + const GIM_CONTACT* scontact = &contacts[keycontacts[i].m_value]; - if(last_key == key)//same points + if (last_key == key) //same points { //merge contact - if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) + if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth) //) { *pcontact = *scontact; - coincident_count = 0; + coincident_count = 0; } - else if(normal_contact_average) + else if (normal_contact_average) { - if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON) - { - if(coincident_count<MAX_COINCIDENT) - { - coincident_normals[coincident_count] = scontact->m_normal; - coincident_count++; - } - } + if (btFabs(pcontact->m_depth - scontact->m_depth) < CONTACT_DIFF_EPSILON) + { + if (coincident_count < MAX_COINCIDENT) + { + coincident_normals[coincident_count] = scontact->m_normal; + coincident_count++; + } + } } } else - {//add new contact + { //add new contact - if(normal_contact_average && coincident_count>0) - { - pcontact->interpolate_normals(coincident_normals,coincident_count); - coincident_count = 0; - } + if (normal_contact_average && coincident_count > 0) + { + pcontact->interpolate_normals(coincident_normals, coincident_count); + coincident_count = 0; + } - push_back(*scontact); - pcontact = &back(); - } + push_back(*scontact); + pcontact = &back(); + } last_key = key; } } -void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts) +void gim_contact_array::merge_contacts_unique(const gim_contact_array& contacts) { clear(); - if(contacts.size()==1) + if (contacts.size() == 1) { push_back(contacts.back()); return; @@ -125,14 +123,14 @@ void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts GIM_CONTACT average_contact = contacts.back(); - for (GUINT i=1;i<contacts.size() ;i++ ) + for (GUINT i = 1; i < contacts.size(); i++) { average_contact.m_point += contacts[i].m_point; average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth; } //divide - GREAL divide_average = 1.0f/((GREAL)contacts.size()); + GREAL divide_average = 1.0f / ((GREAL)contacts.size()); average_contact.m_point *= divide_average; @@ -141,6 +139,4 @@ void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts average_contact.m_depth = average_contact.m_normal.length(); average_contact.m_normal /= average_contact.m_depth; - } - diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_contact.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_contact.h index b41c714b5f..9deb28a26e 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_contact.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_contact.h @@ -36,7 +36,6 @@ email: projectileman@yahoo.com #include "gim_radixsort.h" #include "gim_array.h" - /** Configuration var for applying interpolation of contact normals */ @@ -58,90 +57,87 @@ Configuration var for applying interpolation of contact normals class GIM_CONTACT { public: - btVector3 m_point; - btVector3 m_normal; - GREAL m_depth;//Positive value indicates interpenetration - GREAL m_distance;//Padding not for use - GUINT m_feature1;//Face number - GUINT m_feature2;//Face number + btVector3 m_point; + btVector3 m_normal; + GREAL m_depth; //Positive value indicates interpenetration + GREAL m_distance; //Padding not for use + GUINT m_feature1; //Face number + GUINT m_feature2; //Face number public: - GIM_CONTACT() - { - } - - GIM_CONTACT(const GIM_CONTACT & contact): - m_point(contact.m_point), - m_normal(contact.m_normal), - m_depth(contact.m_depth), - m_feature1(contact.m_feature1), - m_feature2(contact.m_feature2) - { - m_point = contact.m_point; - m_normal = contact.m_normal; - m_depth = contact.m_depth; - m_feature1 = contact.m_feature1; - m_feature2 = contact.m_feature2; - } - - GIM_CONTACT(const btVector3 &point,const btVector3 & normal, - GREAL depth, GUINT feature1, GUINT feature2): - m_point(point), - m_normal(normal), - m_depth(depth), - m_feature1(feature1), - m_feature2(feature2) - { - } + GIM_CONTACT() + { + } + + GIM_CONTACT(const GIM_CONTACT &contact) : m_point(contact.m_point), + m_normal(contact.m_normal), + m_depth(contact.m_depth), + m_feature1(contact.m_feature1), + m_feature2(contact.m_feature2) + { + m_point = contact.m_point; + m_normal = contact.m_normal; + m_depth = contact.m_depth; + m_feature1 = contact.m_feature1; + m_feature2 = contact.m_feature2; + } + + GIM_CONTACT(const btVector3 &point, const btVector3 &normal, + GREAL depth, GUINT feature1, GUINT feature2) : m_point(point), + m_normal(normal), + m_depth(depth), + m_feature1(feature1), + m_feature2(feature2) + { + } //! Calcs key for coord classification - SIMD_FORCE_INLINE GUINT calc_key_contact() const - { - GINT _coords[] = { - (GINT)(m_point[0]*1000.0f+1.0f), - (GINT)(m_point[1]*1333.0f), - (GINT)(m_point[2]*2133.0f+3.0f)}; - GUINT _hash=0; + SIMD_FORCE_INLINE GUINT calc_key_contact() const + { + GINT _coords[] = { + (GINT)(m_point[0] * 1000.0f + 1.0f), + (GINT)(m_point[1] * 1333.0f), + (GINT)(m_point[2] * 2133.0f + 3.0f)}; + GUINT _hash = 0; GUINT *_uitmp = (GUINT *)(&_coords[0]); _hash = *_uitmp; _uitmp++; - _hash += (*_uitmp)<<4; + _hash += (*_uitmp) << 4; _uitmp++; - _hash += (*_uitmp)<<8; + _hash += (*_uitmp) << 8; return _hash; - } + } - SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,GUINT normal_count) - { - btVector3 vec_sum(m_normal); - for(GUINT i=0;i<normal_count;i++) + SIMD_FORCE_INLINE void interpolate_normals(btVector3 *normals, GUINT normal_count) + { + btVector3 vec_sum(m_normal); + for (GUINT i = 0; i < normal_count; i++) { vec_sum += normals[i]; } GREAL vec_sum_len = vec_sum.length2(); - if(vec_sum_len <CONTACT_DIFF_EPSILON) return; - - GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len) + if (vec_sum_len < CONTACT_DIFF_EPSILON) return; - m_normal = vec_sum*vec_sum_len; - } + GIM_INV_SQRT(vec_sum_len, vec_sum_len); // 1/sqrt(vec_sum_len) + m_normal = vec_sum * vec_sum_len; + } }; #endif -class gim_contact_array:public gim_array<GIM_CONTACT> +class gim_contact_array : public gim_array<GIM_CONTACT> { public: - gim_contact_array():gim_array<GIM_CONTACT>(64) + gim_contact_array() : gim_array<GIM_CONTACT>(64) { } - SIMD_FORCE_INLINE void push_contact(const btVector3 &point,const btVector3 & normal, - GREAL depth, GUINT feature1, GUINT feature2) + SIMD_FORCE_INLINE void push_contact(const btVector3 &point, const btVector3 &normal, + GREAL depth, GUINT feature1, GUINT feature2) { push_back_mem(); - GIM_CONTACT & newele = back(); + GIM_CONTACT &newele = back(); newele.m_point = point; newele.m_normal = normal; newele.m_depth = depth; @@ -150,13 +146,13 @@ public: } SIMD_FORCE_INLINE void push_triangle_contacts( - const GIM_TRIANGLE_CONTACT_DATA & tricontact, - GUINT feature1,GUINT feature2) + const GIM_TRIANGLE_CONTACT_DATA &tricontact, + GUINT feature1, GUINT feature2) { - for(GUINT i = 0;i<tricontact.m_point_count ;i++ ) + for (GUINT i = 0; i < tricontact.m_point_count; i++) { push_back_mem(); - GIM_CONTACT & newele = back(); + GIM_CONTACT &newele = back(); newele.m_point = tricontact.m_points[i]; newele.m_normal = tricontact.m_separating_normal; newele.m_depth = tricontact.m_penetration_depth; @@ -165,8 +161,8 @@ public: } } - void merge_contacts(const gim_contact_array & contacts, bool normal_contact_average = true); - void merge_contacts_unique(const gim_contact_array & contacts); + void merge_contacts(const gim_contact_array &contacts, bool normal_contact_average = true); + void merge_contacts_unique(const gim_contact_array &contacts); }; -#endif // GIM_CONTACT_H_INCLUDED +#endif // GIM_CONTACT_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_geom_types.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_geom_types.h index 6b8f9ea6c2..9dc48f354b 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_geom_types.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_geom_types.h @@ -33,11 +33,8 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_math.h" - - //! Short Integer vector 2D typedef GSHORT vec2s[2]; //! Integer vector 3D @@ -92,6 +89,4 @@ typedef GREAL quatf[4]; //typedef struct _aabb3f aabb3f; - - -#endif // GIM_GEOM_TYPES_H_INCLUDED +#endif // GIM_GEOM_TYPES_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_geometry.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_geometry.h index c67a6991c0..4a7ac3c4d8 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_geometry.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_geometry.h @@ -39,4 +39,4 @@ email: projectileman@yahoo.com #include "gim_box_collision.h" #include "gim_tri_collision.h" -#endif // GIM_VECTOR_H_INCLUDED +#endif // GIM_VECTOR_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_hash_table.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_hash_table.h index e4237c2c57..abf88d3108 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_hash_table.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_hash_table.h @@ -34,53 +34,52 @@ email: projectileman@yahoo.com #include "gim_radixsort.h" - -#define GIM_INVALID_HASH 0xffffffff //!< A very very high value +#define GIM_INVALID_HASH 0xffffffff //!< A very very high value #define GIM_DEFAULT_HASH_TABLE_SIZE 380 #define GIM_DEFAULT_HASH_TABLE_NODE_SIZE 4 #define GIM_HASH_TABLE_GROW_FACTOR 2 -#define GIM_MIN_RADIX_SORT_SIZE 860 //!< calibrated on a PIII +#define GIM_MIN_RADIX_SORT_SIZE 860 //!< calibrated on a PIII -template<typename T> +template <typename T> struct GIM_HASH_TABLE_NODE { - GUINT m_key; - T m_data; - GIM_HASH_TABLE_NODE() - { - } - - GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE & value) - { - m_key = value.m_key; - m_data = value.m_data; - } - - GIM_HASH_TABLE_NODE(GUINT key, const T & data) - { - m_key = key; - m_data = data; - } - - bool operator <(const GIM_HASH_TABLE_NODE<T> & other) const + GUINT m_key; + T m_data; + GIM_HASH_TABLE_NODE() + { + } + + GIM_HASH_TABLE_NODE(const GIM_HASH_TABLE_NODE& value) + { + m_key = value.m_key; + m_data = value.m_data; + } + + GIM_HASH_TABLE_NODE(GUINT key, const T& data) + { + m_key = key; + m_data = data; + } + + bool operator<(const GIM_HASH_TABLE_NODE<T>& other) const { ///inverse order, further objects are first - if(m_key < other.m_key) return true; + if (m_key < other.m_key) return true; return false; } - bool operator >(const GIM_HASH_TABLE_NODE<T> & other) const + bool operator>(const GIM_HASH_TABLE_NODE<T>& other) const { ///inverse order, further objects are first - if(m_key > other.m_key) return true; + if (m_key > other.m_key) return true; return false; } - bool operator ==(const GIM_HASH_TABLE_NODE<T> & other) const + bool operator==(const GIM_HASH_TABLE_NODE<T>& other) const { ///inverse order, further objects are first - if(m_key == other.m_key) return true; + if (m_key == other.m_key) return true; return false; } }; @@ -89,21 +88,19 @@ struct GIM_HASH_TABLE_NODE class GIM_HASH_NODE_GET_KEY { public: - template<class T> - inline GUINT operator()( const T& a) + template <class T> + inline GUINT operator()(const T& a) { return a.m_key; } }; - - ///Macro for comparing the key and the element class GIM_HASH_NODE_CMP_KEY_MACRO { public: - template<class T> - inline int operator() ( const T& a, GUINT key) + template <class T> + inline int operator()(const T& a, GUINT key) { return ((int)(a.m_key - key)); } @@ -113,65 +110,53 @@ public: class GIM_HASH_NODE_CMP_MACRO { public: - template<class T> - inline int operator() ( const T& a, const T& b ) + template <class T> + inline int operator()(const T& a, const T& b) { return ((int)(a.m_key - b.m_key)); } }; - - - - //! Sorting for hash table /*! switch automatically between quicksort and radixsort */ -template<typename T> -void gim_sort_hash_node_array(T * array, GUINT array_count) +template <typename T> +void gim_sort_hash_node_array(T* array, GUINT array_count) { - if(array_count<GIM_MIN_RADIX_SORT_SIZE) - { - gim_heap_sort(array,array_count,GIM_HASH_NODE_CMP_MACRO()); - } - else - { - memcopy_elements_func cmpfunc; - gim_radix_sort(array,array_count,GIM_HASH_NODE_GET_KEY(),cmpfunc); - } + if (array_count < GIM_MIN_RADIX_SORT_SIZE) + { + gim_heap_sort(array, array_count, GIM_HASH_NODE_CMP_MACRO()); + } + else + { + memcopy_elements_func cmpfunc; + gim_radix_sort(array, array_count, GIM_HASH_NODE_GET_KEY(), cmpfunc); + } } - - - - - // Note: assumes long is at least 32 bits. #define GIM_NUM_PRIME 28 static const GUINT gim_prime_list[GIM_NUM_PRIME] = -{ - 53ul, 97ul, 193ul, 389ul, 769ul, - 1543ul, 3079ul, 6151ul, 12289ul, 24593ul, - 49157ul, 98317ul, 196613ul, 393241ul, 786433ul, - 1572869ul, 3145739ul, 6291469ul, 12582917ul, 25165843ul, - 50331653ul, 100663319ul, 201326611ul, 402653189ul, 805306457ul, - 1610612741ul, 3221225473ul, 4294967291ul -}; + { + 53ul, 97ul, 193ul, 389ul, 769ul, + 1543ul, 3079ul, 6151ul, 12289ul, 24593ul, + 49157ul, 98317ul, 196613ul, 393241ul, 786433ul, + 1572869ul, 3145739ul, 6291469ul, 12582917ul, 25165843ul, + 50331653ul, 100663319ul, 201326611ul, 402653189ul, 805306457ul, + 1610612741ul, 3221225473ul, 4294967291ul}; inline GUINT gim_next_prime(GUINT number) { - //Find nearest upper prime - GUINT result_ind = 0; - gim_binary_search(gim_prime_list,0,(GIM_NUM_PRIME-2),number,result_ind); + //Find nearest upper prime + GUINT result_ind = 0; + gim_binary_search(gim_prime_list, 0, (GIM_NUM_PRIME - 2), number, result_ind); - // inv: result_ind < 28 - return gim_prime_list[result_ind]; + // inv: result_ind < 28 + return gim_prime_list[result_ind]; } - - //! A compact hash table implementation /*! A memory aligned compact hash table that coud be treated as an array. @@ -187,129 +172,124 @@ When the array size reaches the size equivalent to 'min_hash_table_size', then i </ul> */ -template<class T> +template <class T> class gim_hash_table { protected: - typedef GIM_HASH_TABLE_NODE<T> _node_type; - - //!The nodes - //array< _node_type, SuperAllocator<_node_type> > m_nodes; - gim_array< _node_type > m_nodes; - //SuperBufferedArray< _node_type > m_nodes; - bool m_sorted; - - ///Hash table data management. The hash table has the indices to the corresponding m_nodes array - GUINT * m_hash_table;//!< - GUINT m_table_size;//!< - GUINT m_node_size;//!< - GUINT m_min_hash_table_size; - - - - //! Returns the cell index - inline GUINT _find_cell(GUINT hashkey) - { - _node_type * nodesptr = m_nodes.pointer(); - GUINT start_index = (hashkey%m_table_size)*m_node_size; - GUINT end_index = start_index + m_node_size; - - while(start_index<end_index) - { - GUINT value = m_hash_table[start_index]; - if(value != GIM_INVALID_HASH) - { - if(nodesptr[value].m_key == hashkey) return start_index; - } - start_index++; - } - return GIM_INVALID_HASH; - } - - //! Find the avaliable cell for the hashkey, and return an existing cell if it has the same hash key - inline GUINT _find_avaliable_cell(GUINT hashkey) - { - _node_type * nodesptr = m_nodes.pointer(); - GUINT avaliable_index = GIM_INVALID_HASH; - GUINT start_index = (hashkey%m_table_size)*m_node_size; - GUINT end_index = start_index + m_node_size; - - while(start_index<end_index) - { - GUINT value = m_hash_table[start_index]; - if(value == GIM_INVALID_HASH) - { - if(avaliable_index==GIM_INVALID_HASH) - { - avaliable_index = start_index; - } - } - else if(nodesptr[value].m_key == hashkey) - { - return start_index; - } - start_index++; - } - return avaliable_index; - } - - - - //! reserves the memory for the hash table. - /*! + typedef GIM_HASH_TABLE_NODE<T> _node_type; + + //!The nodes + //array< _node_type, SuperAllocator<_node_type> > m_nodes; + gim_array<_node_type> m_nodes; + //SuperBufferedArray< _node_type > m_nodes; + bool m_sorted; + + ///Hash table data management. The hash table has the indices to the corresponding m_nodes array + GUINT* m_hash_table; //!< + GUINT m_table_size; //!< + GUINT m_node_size; //!< + GUINT m_min_hash_table_size; + + //! Returns the cell index + inline GUINT _find_cell(GUINT hashkey) + { + _node_type* nodesptr = m_nodes.pointer(); + GUINT start_index = (hashkey % m_table_size) * m_node_size; + GUINT end_index = start_index + m_node_size; + + while (start_index < end_index) + { + GUINT value = m_hash_table[start_index]; + if (value != GIM_INVALID_HASH) + { + if (nodesptr[value].m_key == hashkey) return start_index; + } + start_index++; + } + return GIM_INVALID_HASH; + } + + //! Find the avaliable cell for the hashkey, and return an existing cell if it has the same hash key + inline GUINT _find_avaliable_cell(GUINT hashkey) + { + _node_type* nodesptr = m_nodes.pointer(); + GUINT avaliable_index = GIM_INVALID_HASH; + GUINT start_index = (hashkey % m_table_size) * m_node_size; + GUINT end_index = start_index + m_node_size; + + while (start_index < end_index) + { + GUINT value = m_hash_table[start_index]; + if (value == GIM_INVALID_HASH) + { + if (avaliable_index == GIM_INVALID_HASH) + { + avaliable_index = start_index; + } + } + else if (nodesptr[value].m_key == hashkey) + { + return start_index; + } + start_index++; + } + return avaliable_index; + } + + //! reserves the memory for the hash table. + /*! \pre hash table must be empty \post reserves the memory for the hash table, an initializes all elements to GIM_INVALID_HASH. */ - inline void _reserve_table_memory(GUINT newtablesize) - { - if(newtablesize==0) return; - if(m_node_size==0) return; - - //Get a Prime size - - m_table_size = gim_next_prime(newtablesize); - - GUINT datasize = m_table_size*m_node_size; - //Alloc the data buffer - m_hash_table = (GUINT *)gim_alloc(datasize*sizeof(GUINT)); - } - - inline void _invalidate_keys() - { - GUINT datasize = m_table_size*m_node_size; - for(GUINT i=0;i<datasize;i++) - { - m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys - } - } - - //! Clear all memory for the hash table - inline void _clear_table_memory() - { - if(m_hash_table==NULL) return; - gim_free(m_hash_table); - m_hash_table = NULL; - m_table_size = 0; - } - - //! Invalidates the keys (Assigning GIM_INVALID_HASH to all) Reorders the hash keys - inline void _rehash() - { - _invalidate_keys(); - - _node_type * nodesptr = m_nodes.pointer(); - for(GUINT i=0;i<(GUINT)m_nodes.size();i++) - { - GUINT nodekey = nodesptr[i].m_key; - if(nodekey != GIM_INVALID_HASH) - { - //Search for the avaliable cell in buffer - GUINT index = _find_avaliable_cell(nodekey); - - - if(m_hash_table[index]!=GIM_INVALID_HASH) - {//The new index is alreade used... discard this new incomming object, repeated key - btAssert(m_hash_table[index]==nodekey); + inline void _reserve_table_memory(GUINT newtablesize) + { + if (newtablesize == 0) return; + if (m_node_size == 0) return; + + //Get a Prime size + + m_table_size = gim_next_prime(newtablesize); + + GUINT datasize = m_table_size * m_node_size; + //Alloc the data buffer + m_hash_table = (GUINT*)gim_alloc(datasize * sizeof(GUINT)); + } + + inline void _invalidate_keys() + { + GUINT datasize = m_table_size * m_node_size; + for (GUINT i = 0; i < datasize; i++) + { + m_hash_table[i] = GIM_INVALID_HASH; // invalidate keys + } + } + + //! Clear all memory for the hash table + inline void _clear_table_memory() + { + if (m_hash_table == NULL) return; + gim_free(m_hash_table); + m_hash_table = NULL; + m_table_size = 0; + } + + //! Invalidates the keys (Assigning GIM_INVALID_HASH to all) Reorders the hash keys + inline void _rehash() + { + _invalidate_keys(); + + _node_type* nodesptr = m_nodes.pointer(); + for (GUINT i = 0; i < (GUINT)m_nodes.size(); i++) + { + GUINT nodekey = nodesptr[i].m_key; + if (nodekey != GIM_INVALID_HASH) + { + //Search for the avaliable cell in buffer + GUINT index = _find_avaliable_cell(nodekey); + + if (m_hash_table[index] != GIM_INVALID_HASH) + { //The new index is alreade used... discard this new incomming object, repeated key + btAssert(m_hash_table[index] == nodekey); nodesptr[i].m_key = GIM_INVALID_HASH; } else @@ -318,585 +298,560 @@ protected: //Assign the value for alloc m_hash_table[index] = i; } - } - } - } - - //! Resize hash table indices - inline void _resize_table(GUINT newsize) - { - //Clear memory - _clear_table_memory(); - //Alloc the data - _reserve_table_memory(newsize); - //Invalidate keys and rehash - _rehash(); - } - - //! Destroy hash table memory - inline void _destroy() - { - if(m_hash_table==NULL) return; - _clear_table_memory(); - } - - //! Finds an avaliable hash table cell, and resizes the table if there isn't space - inline GUINT _assign_hash_table_cell(GUINT hashkey) - { - GUINT cell_index = _find_avaliable_cell(hashkey); - - if(cell_index==GIM_INVALID_HASH) - { - //rehashing - _resize_table(m_table_size+1); - GUINT cell_index = _find_avaliable_cell(hashkey); - btAssert(cell_index!=GIM_INVALID_HASH); - } - return cell_index; - } - - //! erase by index in hash table - inline bool _erase_by_index_hash_table(GUINT index) - { - if(index >= m_nodes.size()) return false; - if(m_nodes[index].m_key != GIM_INVALID_HASH) - { - //Search for the avaliable cell in buffer - GUINT cell_index = _find_cell(m_nodes[index].m_key); - - btAssert(cell_index!=GIM_INVALID_HASH); - btAssert(m_hash_table[cell_index]==index); - - m_hash_table[cell_index] = GIM_INVALID_HASH; - } - - return this->_erase_unsorted(index); - } - - //! erase by key in hash table - inline bool _erase_hash_table(GUINT hashkey) - { - if(hashkey == GIM_INVALID_HASH) return false; - - //Search for the avaliable cell in buffer - GUINT cell_index = _find_cell(hashkey); - if(cell_index ==GIM_INVALID_HASH) return false; - - GUINT index = m_hash_table[cell_index]; - m_hash_table[cell_index] = GIM_INVALID_HASH; - - return this->_erase_unsorted(index); - } - - - - //! insert an element in hash table - /*! + } + } + } + + //! Resize hash table indices + inline void _resize_table(GUINT newsize) + { + //Clear memory + _clear_table_memory(); + //Alloc the data + _reserve_table_memory(newsize); + //Invalidate keys and rehash + _rehash(); + } + + //! Destroy hash table memory + inline void _destroy() + { + if (m_hash_table == NULL) return; + _clear_table_memory(); + } + + //! Finds an avaliable hash table cell, and resizes the table if there isn't space + inline GUINT _assign_hash_table_cell(GUINT hashkey) + { + GUINT cell_index = _find_avaliable_cell(hashkey); + + if (cell_index == GIM_INVALID_HASH) + { + //rehashing + _resize_table(m_table_size + 1); + GUINT cell_index = _find_avaliable_cell(hashkey); + btAssert(cell_index != GIM_INVALID_HASH); + } + return cell_index; + } + + //! erase by index in hash table + inline bool _erase_by_index_hash_table(GUINT index) + { + if (index >= m_nodes.size()) return false; + if (m_nodes[index].m_key != GIM_INVALID_HASH) + { + //Search for the avaliable cell in buffer + GUINT cell_index = _find_cell(m_nodes[index].m_key); + + btAssert(cell_index != GIM_INVALID_HASH); + btAssert(m_hash_table[cell_index] == index); + + m_hash_table[cell_index] = GIM_INVALID_HASH; + } + + return this->_erase_unsorted(index); + } + + //! erase by key in hash table + inline bool _erase_hash_table(GUINT hashkey) + { + if (hashkey == GIM_INVALID_HASH) return false; + + //Search for the avaliable cell in buffer + GUINT cell_index = _find_cell(hashkey); + if (cell_index == GIM_INVALID_HASH) return false; + + GUINT index = m_hash_table[cell_index]; + m_hash_table[cell_index] = GIM_INVALID_HASH; + + return this->_erase_unsorted(index); + } + + //! insert an element in hash table + /*! If the element exists, this won't insert the element \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted If so, the element has been inserted at the last position of the array. */ - inline GUINT _insert_hash_table(GUINT hashkey, const T & value) - { - if(hashkey==GIM_INVALID_HASH) - { - //Insert anyway - _insert_unsorted(hashkey,value); - return GIM_INVALID_HASH; - } + inline GUINT _insert_hash_table(GUINT hashkey, const T& value) + { + if (hashkey == GIM_INVALID_HASH) + { + //Insert anyway + _insert_unsorted(hashkey, value); + return GIM_INVALID_HASH; + } - GUINT cell_index = _assign_hash_table_cell(hashkey); + GUINT cell_index = _assign_hash_table_cell(hashkey); - GUINT value_key = m_hash_table[cell_index]; + GUINT value_key = m_hash_table[cell_index]; - if(value_key!= GIM_INVALID_HASH) return value_key;// Not overrited + if (value_key != GIM_INVALID_HASH) return value_key; // Not overrited - m_hash_table[cell_index] = m_nodes.size(); + m_hash_table[cell_index] = m_nodes.size(); - _insert_unsorted(hashkey,value); - return GIM_INVALID_HASH; - } + _insert_unsorted(hashkey, value); + return GIM_INVALID_HASH; + } - //! insert an element in hash table. - /*! + //! insert an element in hash table. + /*! If the element exists, this replaces the element. \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted If so, the element has been inserted at the last position of the array. */ - inline GUINT _insert_hash_table_replace(GUINT hashkey, const T & value) - { - if(hashkey==GIM_INVALID_HASH) - { - //Insert anyway - _insert_unsorted(hashkey,value); - return GIM_INVALID_HASH; - } - - GUINT cell_index = _assign_hash_table_cell(hashkey); - - GUINT value_key = m_hash_table[cell_index]; - - if(value_key!= GIM_INVALID_HASH) - {//replaces the existing - m_nodes[value_key] = _node_type(hashkey,value); - return value_key;// index of the replaced element - } - - m_hash_table[cell_index] = m_nodes.size(); - - _insert_unsorted(hashkey,value); - return GIM_INVALID_HASH; - - } - - - ///Sorted array data management. The hash table has the indices to the corresponding m_nodes array - inline bool _erase_sorted(GUINT index) - { - if(index>=(GUINT)m_nodes.size()) return false; - m_nodes.erase_sorted(index); - if(m_nodes.size()<2) m_sorted = false; - return true; - } - - //! faster, but unsorted - inline bool _erase_unsorted(GUINT index) - { - if(index>=m_nodes.size()) return false; - - GUINT lastindex = m_nodes.size()-1; - if(index<lastindex && m_hash_table!=0) - { - GUINT hashkey = m_nodes[lastindex].m_key; - if(hashkey!=GIM_INVALID_HASH) + inline GUINT _insert_hash_table_replace(GUINT hashkey, const T& value) + { + if (hashkey == GIM_INVALID_HASH) + { + //Insert anyway + _insert_unsorted(hashkey, value); + return GIM_INVALID_HASH; + } + + GUINT cell_index = _assign_hash_table_cell(hashkey); + + GUINT value_key = m_hash_table[cell_index]; + + if (value_key != GIM_INVALID_HASH) + { //replaces the existing + m_nodes[value_key] = _node_type(hashkey, value); + return value_key; // index of the replaced element + } + + m_hash_table[cell_index] = m_nodes.size(); + + _insert_unsorted(hashkey, value); + return GIM_INVALID_HASH; + } + + ///Sorted array data management. The hash table has the indices to the corresponding m_nodes array + inline bool _erase_sorted(GUINT index) + { + if (index >= (GUINT)m_nodes.size()) return false; + m_nodes.erase_sorted(index); + if (m_nodes.size() < 2) m_sorted = false; + return true; + } + + //! faster, but unsorted + inline bool _erase_unsorted(GUINT index) + { + if (index >= m_nodes.size()) return false; + + GUINT lastindex = m_nodes.size() - 1; + if (index < lastindex && m_hash_table != 0) + { + GUINT hashkey = m_nodes[lastindex].m_key; + if (hashkey != GIM_INVALID_HASH) { //update the new position of the last element GUINT cell_index = _find_cell(hashkey); - btAssert(cell_index!=GIM_INVALID_HASH); + btAssert(cell_index != GIM_INVALID_HASH); //new position of the last element which will be swaped m_hash_table[cell_index] = index; } - } - m_nodes.erase(index); - m_sorted = false; - return true; - } - - //! Insert in position ordered - /*! + } + m_nodes.erase(index); + m_sorted = false; + return true; + } + + //! Insert in position ordered + /*! Also checks if it is needed to transform this container to a hash table, by calling check_for_switching_to_hashtable */ - inline void _insert_in_pos(GUINT hashkey, const T & value, GUINT pos) - { - m_nodes.insert(_node_type(hashkey,value),pos); - this->check_for_switching_to_hashtable(); - } - - //! Insert an element in an ordered array - inline GUINT _insert_sorted(GUINT hashkey, const T & value) - { - if(hashkey==GIM_INVALID_HASH || size()==0) - { - m_nodes.push_back(_node_type(hashkey,value)); - return GIM_INVALID_HASH; - } - //Insert at last position - //Sort element - - - GUINT result_ind=0; - GUINT last_index = m_nodes.size()-1; - _node_type * ptr = m_nodes.pointer(); - - bool found = gim_binary_search_ex( - ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO()); - - - //Insert before found index - if(found) - { - return result_ind; - } - else - { - _insert_in_pos(hashkey, value, result_ind); - } - return GIM_INVALID_HASH; - } - - inline GUINT _insert_sorted_replace(GUINT hashkey, const T & value) - { - if(hashkey==GIM_INVALID_HASH || size()==0) - { - m_nodes.push_back(_node_type(hashkey,value)); - return GIM_INVALID_HASH; - } - //Insert at last position - //Sort element - GUINT result_ind; - GUINT last_index = m_nodes.size()-1; - _node_type * ptr = m_nodes.pointer(); - - bool found = gim_binary_search_ex( - ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO()); - - //Insert before found index - if(found) - { - m_nodes[result_ind] = _node_type(hashkey,value); - } - else - { - _insert_in_pos(hashkey, value, result_ind); - } - return result_ind; - } - - //! Fast insertion in m_nodes array - inline GUINT _insert_unsorted(GUINT hashkey, const T & value) - { - m_nodes.push_back(_node_type(hashkey,value)); - m_sorted = false; - return GIM_INVALID_HASH; - } - - + inline void _insert_in_pos(GUINT hashkey, const T& value, GUINT pos) + { + m_nodes.insert(_node_type(hashkey, value), pos); + this->check_for_switching_to_hashtable(); + } -public: + //! Insert an element in an ordered array + inline GUINT _insert_sorted(GUINT hashkey, const T& value) + { + if (hashkey == GIM_INVALID_HASH || size() == 0) + { + m_nodes.push_back(_node_type(hashkey, value)); + return GIM_INVALID_HASH; + } + //Insert at last position + //Sort element + + GUINT result_ind = 0; + GUINT last_index = m_nodes.size() - 1; + _node_type* ptr = m_nodes.pointer(); + + bool found = gim_binary_search_ex( + ptr, 0, last_index, result_ind, hashkey, GIM_HASH_NODE_CMP_KEY_MACRO()); + + //Insert before found index + if (found) + { + return result_ind; + } + else + { + _insert_in_pos(hashkey, value, result_ind); + } + return GIM_INVALID_HASH; + } - /*! + inline GUINT _insert_sorted_replace(GUINT hashkey, const T& value) + { + if (hashkey == GIM_INVALID_HASH || size() == 0) + { + m_nodes.push_back(_node_type(hashkey, value)); + return GIM_INVALID_HASH; + } + //Insert at last position + //Sort element + GUINT result_ind; + GUINT last_index = m_nodes.size() - 1; + _node_type* ptr = m_nodes.pointer(); + + bool found = gim_binary_search_ex( + ptr, 0, last_index, result_ind, hashkey, GIM_HASH_NODE_CMP_KEY_MACRO()); + + //Insert before found index + if (found) + { + m_nodes[result_ind] = _node_type(hashkey, value); + } + else + { + _insert_in_pos(hashkey, value, result_ind); + } + return result_ind; + } + + //! Fast insertion in m_nodes array + inline GUINT _insert_unsorted(GUINT hashkey, const T& value) + { + m_nodes.push_back(_node_type(hashkey, value)); + m_sorted = false; + return GIM_INVALID_HASH; + } + +public: + /*! <li> if node_size = 0, then this container becomes a simple sorted array allocator. reserve_size is used for reserve memory in m_nodes. When the array size reaches the size equivalent to 'min_hash_table_size', then it becomes a hash table by calling check_for_switching_to_hashtable. <li> If node_size != 0, then this container becomes a hash table for ever </ul> */ - gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE, - GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE, - GUINT min_hash_table_size = GIM_INVALID_HASH) - { - m_hash_table = NULL; - m_table_size = 0; - m_sorted = false; - m_node_size = node_size; - m_min_hash_table_size = min_hash_table_size; - - if(m_node_size!=0) - { - if(reserve_size!=0) - { - m_nodes.reserve(reserve_size); - _reserve_table_memory(reserve_size); - _invalidate_keys(); - } - else - { - m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE); - _reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE); - _invalidate_keys(); - } - } - else if(reserve_size!=0) - { - m_nodes.reserve(reserve_size); - } - - } - - ~gim_hash_table() - { - _destroy(); - } - - inline bool is_hash_table() - { - if(m_hash_table) return true; - return false; - } - - inline bool is_sorted() - { - if(size()<2) return true; - return m_sorted; - } - - bool sort() - { - if(is_sorted()) return true; - if(m_nodes.size()<2) return false; - - - _node_type * ptr = m_nodes.pointer(); - GUINT siz = m_nodes.size(); - gim_sort_hash_node_array(ptr,siz); - m_sorted=true; - - - - if(m_hash_table) - { - _rehash(); - } - return true; - } - - bool switch_to_hashtable() - { - if(m_hash_table) return false; - if(m_node_size==0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE; - if(m_nodes.size()<GIM_DEFAULT_HASH_TABLE_SIZE) - { - _resize_table(GIM_DEFAULT_HASH_TABLE_SIZE); - } - else - { - _resize_table(m_nodes.size()+1); - } - - return true; - } - - bool switch_to_sorted_array() - { - if(m_hash_table==NULL) return true; - _clear_table_memory(); - return sort(); - } - - //!If the container reaches the - bool check_for_switching_to_hashtable() - { - if(this->m_hash_table) return true; - - if(!(m_nodes.size()< m_min_hash_table_size)) - { - if(m_node_size == 0) - { - m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE; - } - - _resize_table(m_nodes.size()+1); - return true; - } - return false; - } - - inline void set_sorted(bool value) - { - m_sorted = value; - } - - //! Retrieves the amount of keys. - inline GUINT size() const - { - return m_nodes.size(); - } - - //! Retrieves the hash key. - inline GUINT get_key(GUINT index) const - { - return m_nodes[index].m_key; - } - - //! Retrieves the value by index - /*! + gim_hash_table(GUINT reserve_size = GIM_DEFAULT_HASH_TABLE_SIZE, + GUINT node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE, + GUINT min_hash_table_size = GIM_INVALID_HASH) + { + m_hash_table = NULL; + m_table_size = 0; + m_sorted = false; + m_node_size = node_size; + m_min_hash_table_size = min_hash_table_size; + + if (m_node_size != 0) + { + if (reserve_size != 0) + { + m_nodes.reserve(reserve_size); + _reserve_table_memory(reserve_size); + _invalidate_keys(); + } + else + { + m_nodes.reserve(GIM_DEFAULT_HASH_TABLE_SIZE); + _reserve_table_memory(GIM_DEFAULT_HASH_TABLE_SIZE); + _invalidate_keys(); + } + } + else if (reserve_size != 0) + { + m_nodes.reserve(reserve_size); + } + } + + ~gim_hash_table() + { + _destroy(); + } + + inline bool is_hash_table() + { + if (m_hash_table) return true; + return false; + } + + inline bool is_sorted() + { + if (size() < 2) return true; + return m_sorted; + } + + bool sort() + { + if (is_sorted()) return true; + if (m_nodes.size() < 2) return false; + + _node_type* ptr = m_nodes.pointer(); + GUINT siz = m_nodes.size(); + gim_sort_hash_node_array(ptr, siz); + m_sorted = true; + + if (m_hash_table) + { + _rehash(); + } + return true; + } + + bool switch_to_hashtable() + { + if (m_hash_table) return false; + if (m_node_size == 0) m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE; + if (m_nodes.size() < GIM_DEFAULT_HASH_TABLE_SIZE) + { + _resize_table(GIM_DEFAULT_HASH_TABLE_SIZE); + } + else + { + _resize_table(m_nodes.size() + 1); + } + + return true; + } + + bool switch_to_sorted_array() + { + if (m_hash_table == NULL) return true; + _clear_table_memory(); + return sort(); + } + + //!If the container reaches the + bool check_for_switching_to_hashtable() + { + if (this->m_hash_table) return true; + + if (!(m_nodes.size() < m_min_hash_table_size)) + { + if (m_node_size == 0) + { + m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE; + } + + _resize_table(m_nodes.size() + 1); + return true; + } + return false; + } + + inline void set_sorted(bool value) + { + m_sorted = value; + } + + //! Retrieves the amount of keys. + inline GUINT size() const + { + return m_nodes.size(); + } + + //! Retrieves the hash key. + inline GUINT get_key(GUINT index) const + { + return m_nodes[index].m_key; + } + + //! Retrieves the value by index + /*! */ - inline T * get_value_by_index(GUINT index) - { - return &m_nodes[index].m_data; - } - - inline const T& operator[](GUINT index) const - { - return m_nodes[index].m_data; - } - - inline T& operator[](GUINT index) - { - return m_nodes[index].m_data; - } - - //! Finds the index of the element with the key - /*! + inline T* get_value_by_index(GUINT index) + { + return &m_nodes[index].m_data; + } + + inline const T& operator[](GUINT index) const + { + return m_nodes[index].m_data; + } + + inline T& operator[](GUINT index) + { + return m_nodes[index].m_data; + } + + //! Finds the index of the element with the key + /*! \return the index in the array of the existing element,or GIM_INVALID_HASH if the element has been inserted If so, the element has been inserted at the last position of the array. */ - inline GUINT find(GUINT hashkey) - { - if(m_hash_table) - { - GUINT cell_index = _find_cell(hashkey); - if(cell_index==GIM_INVALID_HASH) return GIM_INVALID_HASH; - return m_hash_table[cell_index]; - } + inline GUINT find(GUINT hashkey) + { + if (m_hash_table) + { + GUINT cell_index = _find_cell(hashkey); + if (cell_index == GIM_INVALID_HASH) return GIM_INVALID_HASH; + return m_hash_table[cell_index]; + } GUINT last_index = m_nodes.size(); - if(last_index<2) - { - if(last_index==0) return GIM_INVALID_HASH; - if(m_nodes[0].m_key == hashkey) return 0; - return GIM_INVALID_HASH; - } - else if(m_sorted) - { - //Binary search - GUINT result_ind = 0; + if (last_index < 2) + { + if (last_index == 0) return GIM_INVALID_HASH; + if (m_nodes[0].m_key == hashkey) return 0; + return GIM_INVALID_HASH; + } + else if (m_sorted) + { + //Binary search + GUINT result_ind = 0; last_index--; - _node_type * ptr = m_nodes.pointer(); + _node_type* ptr = m_nodes.pointer(); - bool found = gim_binary_search_ex(ptr,0,last_index,result_ind,hashkey,GIM_HASH_NODE_CMP_KEY_MACRO()); + bool found = gim_binary_search_ex(ptr, 0, last_index, result_ind, hashkey, GIM_HASH_NODE_CMP_KEY_MACRO()); + if (found) return result_ind; + } + return GIM_INVALID_HASH; + } - if(found) return result_ind; - } - return GIM_INVALID_HASH; - } - - //! Retrieves the value associated with the index - /*! + //! Retrieves the value associated with the index + /*! \return the found element, or null */ - inline T * get_value(GUINT hashkey) - { - GUINT index = find(hashkey); - if(index == GIM_INVALID_HASH) return NULL; - return &m_nodes[index].m_data; - } - + inline T* get_value(GUINT hashkey) + { + GUINT index = find(hashkey); + if (index == GIM_INVALID_HASH) return NULL; + return &m_nodes[index].m_data; + } - /*! + /*! */ - inline bool erase_by_index(GUINT index) - { - if(index > m_nodes.size()) return false; - - if(m_hash_table == NULL) - { - if(is_sorted()) - { - return this->_erase_sorted(index); - } - else - { - return this->_erase_unsorted(index); - } - } - else - { - return this->_erase_by_index_hash_table(index); - } - return false; - } - - - - inline bool erase_by_index_unsorted(GUINT index) - { - if(index > m_nodes.size()) return false; - - if(m_hash_table == NULL) - { - return this->_erase_unsorted(index); - } - else - { - return this->_erase_by_index_hash_table(index); - } - return false; - } - - - - /*! + inline bool erase_by_index(GUINT index) + { + if (index > m_nodes.size()) return false; + + if (m_hash_table == NULL) + { + if (is_sorted()) + { + return this->_erase_sorted(index); + } + else + { + return this->_erase_unsorted(index); + } + } + else + { + return this->_erase_by_index_hash_table(index); + } + return false; + } + + inline bool erase_by_index_unsorted(GUINT index) + { + if (index > m_nodes.size()) return false; + + if (m_hash_table == NULL) + { + return this->_erase_unsorted(index); + } + else + { + return this->_erase_by_index_hash_table(index); + } + return false; + } + + /*! */ - inline bool erase_by_key(GUINT hashkey) - { - if(size()==0) return false; - - if(m_hash_table) - { - return this->_erase_hash_table(hashkey); - } - //Binary search - - if(is_sorted()==false) return false; - - GUINT result_ind = find(hashkey); - if(result_ind!= GIM_INVALID_HASH) - { - return this->_erase_sorted(result_ind); - } - return false; - } - - void clear() - { - m_nodes.clear(); - - if(m_hash_table==NULL) return; - GUINT datasize = m_table_size*m_node_size; - //Initialize the hashkeys. - GUINT i; - for(i=0;i<datasize;i++) - { - m_hash_table[i] = GIM_INVALID_HASH;// invalidate keys - } + inline bool erase_by_key(GUINT hashkey) + { + if (size() == 0) return false; + + if (m_hash_table) + { + return this->_erase_hash_table(hashkey); + } + //Binary search + + if (is_sorted() == false) return false; + + GUINT result_ind = find(hashkey); + if (result_ind != GIM_INVALID_HASH) + { + return this->_erase_sorted(result_ind); + } + return false; + } + + void clear() + { + m_nodes.clear(); + + if (m_hash_table == NULL) return; + GUINT datasize = m_table_size * m_node_size; + //Initialize the hashkeys. + GUINT i; + for (i = 0; i < datasize; i++) + { + m_hash_table[i] = GIM_INVALID_HASH; // invalidate keys + } m_sorted = false; - } + } - //! Insert an element into the hash - /*! + //! Insert an element into the hash + /*! \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position of the existing element. */ - inline GUINT insert(GUINT hashkey, const T & element) - { - if(m_hash_table) - { - return this->_insert_hash_table(hashkey,element); - } - if(this->is_sorted()) - { - return this->_insert_sorted(hashkey,element); - } - return this->_insert_unsorted(hashkey,element); - } - - //! Insert an element into the hash, and could overrite an existing object with the same hash. - /*! + inline GUINT insert(GUINT hashkey, const T& element) + { + if (m_hash_table) + { + return this->_insert_hash_table(hashkey, element); + } + if (this->is_sorted()) + { + return this->_insert_sorted(hashkey, element); + } + return this->_insert_unsorted(hashkey, element); + } + + //! Insert an element into the hash, and could overrite an existing object with the same hash. + /*! \return If GIM_INVALID_HASH, the object has been inserted succesfully. Else it returns the position of the replaced element. */ - inline GUINT insert_override(GUINT hashkey, const T & element) - { - if(m_hash_table) - { - return this->_insert_hash_table_replace(hashkey,element); - } - if(this->is_sorted()) - { - return this->_insert_sorted_replace(hashkey,element); - } - this->_insert_unsorted(hashkey,element); - return m_nodes.size(); - } - - - - //! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted - /*! - */ - inline GUINT insert_unsorted(GUINT hashkey,const T & element) - { - if(m_hash_table) - { - return this->_insert_hash_table(hashkey,element); - } - return this->_insert_unsorted(hashkey,element); - } - + inline GUINT insert_override(GUINT hashkey, const T& element) + { + if (m_hash_table) + { + return this->_insert_hash_table_replace(hashkey, element); + } + if (this->is_sorted()) + { + return this->_insert_sorted_replace(hashkey, element); + } + this->_insert_unsorted(hashkey, element); + return m_nodes.size(); + } + //! Insert an element into the hash,But if this container is a sorted array, this inserts it unsorted + /*! + */ + inline GUINT insert_unsorted(GUINT hashkey, const T& element) + { + if (m_hash_table) + { + return this->_insert_hash_table(hashkey, element); + } + return this->_insert_unsorted(hashkey, element); + } }; - - -#endif // GIM_CONTAINERS_H_INCLUDED +#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_linear_math.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_linear_math.h index 64f11b4954..98401a404a 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_linear_math.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_linear_math.h @@ -34,962 +34,900 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_math.h" #include "gim_geom_types.h" - - - //! Zero out a 2D vector -#define VEC_ZERO_2(a) \ -{ \ - (a)[0] = (a)[1] = 0.0f; \ -}\ - +#define VEC_ZERO_2(a) \ + { \ + (a)[0] = (a)[1] = 0.0f; \ + } //! Zero out a 3D vector -#define VEC_ZERO(a) \ -{ \ - (a)[0] = (a)[1] = (a)[2] = 0.0f; \ -}\ - +#define VEC_ZERO(a) \ + { \ + (a)[0] = (a)[1] = (a)[2] = 0.0f; \ + } /// Zero out a 4D vector -#define VEC_ZERO_4(a) \ -{ \ - (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f; \ -}\ - +#define VEC_ZERO_4(a) \ + { \ + (a)[0] = (a)[1] = (a)[2] = (a)[3] = 0.0f; \ + } /// Vector copy -#define VEC_COPY_2(b,a) \ -{ \ - (b)[0] = (a)[0]; \ - (b)[1] = (a)[1]; \ -}\ - +#define VEC_COPY_2(b, a) \ + { \ + (b)[0] = (a)[0]; \ + (b)[1] = (a)[1]; \ + } /// Copy 3D vector -#define VEC_COPY(b,a) \ -{ \ - (b)[0] = (a)[0]; \ - (b)[1] = (a)[1]; \ - (b)[2] = (a)[2]; \ -}\ - +#define VEC_COPY(b, a) \ + { \ + (b)[0] = (a)[0]; \ + (b)[1] = (a)[1]; \ + (b)[2] = (a)[2]; \ + } /// Copy 4D vector -#define VEC_COPY_4(b,a) \ -{ \ - (b)[0] = (a)[0]; \ - (b)[1] = (a)[1]; \ - (b)[2] = (a)[2]; \ - (b)[3] = (a)[3]; \ -}\ +#define VEC_COPY_4(b, a) \ + { \ + (b)[0] = (a)[0]; \ + (b)[1] = (a)[1]; \ + (b)[2] = (a)[2]; \ + (b)[3] = (a)[3]; \ + } /// VECTOR SWAP -#define VEC_SWAP(b,a) \ -{ \ - GIM_SWAP_NUMBERS((b)[0],(a)[0]);\ - GIM_SWAP_NUMBERS((b)[1],(a)[1]);\ - GIM_SWAP_NUMBERS((b)[2],(a)[2]);\ -}\ +#define VEC_SWAP(b, a) \ + { \ + GIM_SWAP_NUMBERS((b)[0], (a)[0]); \ + GIM_SWAP_NUMBERS((b)[1], (a)[1]); \ + GIM_SWAP_NUMBERS((b)[2], (a)[2]); \ + } /// Vector difference -#define VEC_DIFF_2(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] - (v1)[0]; \ - (v21)[1] = (v2)[1] - (v1)[1]; \ -}\ - +#define VEC_DIFF_2(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] - (v1)[0]; \ + (v21)[1] = (v2)[1] - (v1)[1]; \ + } /// Vector difference -#define VEC_DIFF(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] - (v1)[0]; \ - (v21)[1] = (v2)[1] - (v1)[1]; \ - (v21)[2] = (v2)[2] - (v1)[2]; \ -}\ - +#define VEC_DIFF(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] - (v1)[0]; \ + (v21)[1] = (v2)[1] - (v1)[1]; \ + (v21)[2] = (v2)[2] - (v1)[2]; \ + } /// Vector difference -#define VEC_DIFF_4(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] - (v1)[0]; \ - (v21)[1] = (v2)[1] - (v1)[1]; \ - (v21)[2] = (v2)[2] - (v1)[2]; \ - (v21)[3] = (v2)[3] - (v1)[3]; \ -}\ - +#define VEC_DIFF_4(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] - (v1)[0]; \ + (v21)[1] = (v2)[1] - (v1)[1]; \ + (v21)[2] = (v2)[2] - (v1)[2]; \ + (v21)[3] = (v2)[3] - (v1)[3]; \ + } /// Vector sum -#define VEC_SUM_2(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] + (v1)[0]; \ - (v21)[1] = (v2)[1] + (v1)[1]; \ -}\ - +#define VEC_SUM_2(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] + (v1)[0]; \ + (v21)[1] = (v2)[1] + (v1)[1]; \ + } /// Vector sum -#define VEC_SUM(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] + (v1)[0]; \ - (v21)[1] = (v2)[1] + (v1)[1]; \ - (v21)[2] = (v2)[2] + (v1)[2]; \ -}\ - +#define VEC_SUM(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] + (v1)[0]; \ + (v21)[1] = (v2)[1] + (v1)[1]; \ + (v21)[2] = (v2)[2] + (v1)[2]; \ + } /// Vector sum -#define VEC_SUM_4(v21,v2,v1) \ -{ \ - (v21)[0] = (v2)[0] + (v1)[0]; \ - (v21)[1] = (v2)[1] + (v1)[1]; \ - (v21)[2] = (v2)[2] + (v1)[2]; \ - (v21)[3] = (v2)[3] + (v1)[3]; \ -}\ - +#define VEC_SUM_4(v21, v2, v1) \ + { \ + (v21)[0] = (v2)[0] + (v1)[0]; \ + (v21)[1] = (v2)[1] + (v1)[1]; \ + (v21)[2] = (v2)[2] + (v1)[2]; \ + (v21)[3] = (v2)[3] + (v1)[3]; \ + } /// scalar times vector -#define VEC_SCALE_2(c,a,b) \ -{ \ - (c)[0] = (a)*(b)[0]; \ - (c)[1] = (a)*(b)[1]; \ -}\ - +#define VEC_SCALE_2(c, a, b) \ + { \ + (c)[0] = (a) * (b)[0]; \ + (c)[1] = (a) * (b)[1]; \ + } /// scalar times vector -#define VEC_SCALE(c,a,b) \ -{ \ - (c)[0] = (a)*(b)[0]; \ - (c)[1] = (a)*(b)[1]; \ - (c)[2] = (a)*(b)[2]; \ -}\ - +#define VEC_SCALE(c, a, b) \ + { \ + (c)[0] = (a) * (b)[0]; \ + (c)[1] = (a) * (b)[1]; \ + (c)[2] = (a) * (b)[2]; \ + } /// scalar times vector -#define VEC_SCALE_4(c,a,b) \ -{ \ - (c)[0] = (a)*(b)[0]; \ - (c)[1] = (a)*(b)[1]; \ - (c)[2] = (a)*(b)[2]; \ - (c)[3] = (a)*(b)[3]; \ -}\ - +#define VEC_SCALE_4(c, a, b) \ + { \ + (c)[0] = (a) * (b)[0]; \ + (c)[1] = (a) * (b)[1]; \ + (c)[2] = (a) * (b)[2]; \ + (c)[3] = (a) * (b)[3]; \ + } /// accumulate scaled vector -#define VEC_ACCUM_2(c,a,b) \ -{ \ - (c)[0] += (a)*(b)[0]; \ - (c)[1] += (a)*(b)[1]; \ -}\ - +#define VEC_ACCUM_2(c, a, b) \ + { \ + (c)[0] += (a) * (b)[0]; \ + (c)[1] += (a) * (b)[1]; \ + } /// accumulate scaled vector -#define VEC_ACCUM(c,a,b) \ -{ \ - (c)[0] += (a)*(b)[0]; \ - (c)[1] += (a)*(b)[1]; \ - (c)[2] += (a)*(b)[2]; \ -}\ - +#define VEC_ACCUM(c, a, b) \ + { \ + (c)[0] += (a) * (b)[0]; \ + (c)[1] += (a) * (b)[1]; \ + (c)[2] += (a) * (b)[2]; \ + } /// accumulate scaled vector -#define VEC_ACCUM_4(c,a,b) \ -{ \ - (c)[0] += (a)*(b)[0]; \ - (c)[1] += (a)*(b)[1]; \ - (c)[2] += (a)*(b)[2]; \ - (c)[3] += (a)*(b)[3]; \ -}\ - +#define VEC_ACCUM_4(c, a, b) \ + { \ + (c)[0] += (a) * (b)[0]; \ + (c)[1] += (a) * (b)[1]; \ + (c)[2] += (a) * (b)[2]; \ + (c)[3] += (a) * (b)[3]; \ + } /// Vector dot product -#define VEC_DOT_2(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1]) - +#define VEC_DOT_2(a, b) ((a)[0] * (b)[0] + (a)[1] * (b)[1]) /// Vector dot product -#define VEC_DOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]) +#define VEC_DOT(a, b) ((a)[0] * (b)[0] + (a)[1] * (b)[1] + (a)[2] * (b)[2]) /// Vector dot product -#define VEC_DOT_4(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3]) +#define VEC_DOT_4(a, b) ((a)[0] * (b)[0] + (a)[1] * (b)[1] + (a)[2] * (b)[2] + (a)[3] * (b)[3]) /// vector impact parameter (squared) -#define VEC_IMPACT_SQ(bsq,direction,position) {\ - GREAL _llel_ = VEC_DOT(direction, position);\ - bsq = VEC_DOT(position, position) - _llel_*_llel_;\ -}\ - +#define VEC_IMPACT_SQ(bsq, direction, position) \ + { \ + GREAL _llel_ = VEC_DOT(direction, position); \ + bsq = VEC_DOT(position, position) - _llel_ * _llel_; \ + } /// vector impact parameter -#define VEC_IMPACT(bsq,direction,position) {\ - VEC_IMPACT_SQ(bsq,direction,position); \ - GIM_SQRT(bsq,bsq); \ -}\ +#define VEC_IMPACT(bsq, direction, position) \ + { \ + VEC_IMPACT_SQ(bsq, direction, position); \ + GIM_SQRT(bsq, bsq); \ + } /// Vector length -#define VEC_LENGTH_2(a,l)\ -{\ - GREAL _pp = VEC_DOT_2(a,a);\ - GIM_SQRT(_pp,l);\ -}\ - +#define VEC_LENGTH_2(a, l) \ + { \ + GREAL _pp = VEC_DOT_2(a, a); \ + GIM_SQRT(_pp, l); \ + } /// Vector length -#define VEC_LENGTH(a,l)\ -{\ - GREAL _pp = VEC_DOT(a,a);\ - GIM_SQRT(_pp,l);\ -}\ - +#define VEC_LENGTH(a, l) \ + { \ + GREAL _pp = VEC_DOT(a, a); \ + GIM_SQRT(_pp, l); \ + } /// Vector length -#define VEC_LENGTH_4(a,l)\ -{\ - GREAL _pp = VEC_DOT_4(a,a);\ - GIM_SQRT(_pp,l);\ -}\ +#define VEC_LENGTH_4(a, l) \ + { \ + GREAL _pp = VEC_DOT_4(a, a); \ + GIM_SQRT(_pp, l); \ + } /// Vector inv length -#define VEC_INV_LENGTH_2(a,l)\ -{\ - GREAL _pp = VEC_DOT_2(a,a);\ - GIM_INV_SQRT(_pp,l);\ -}\ - +#define VEC_INV_LENGTH_2(a, l) \ + { \ + GREAL _pp = VEC_DOT_2(a, a); \ + GIM_INV_SQRT(_pp, l); \ + } /// Vector inv length -#define VEC_INV_LENGTH(a,l)\ -{\ - GREAL _pp = VEC_DOT(a,a);\ - GIM_INV_SQRT(_pp,l);\ -}\ - +#define VEC_INV_LENGTH(a, l) \ + { \ + GREAL _pp = VEC_DOT(a, a); \ + GIM_INV_SQRT(_pp, l); \ + } /// Vector inv length -#define VEC_INV_LENGTH_4(a,l)\ -{\ - GREAL _pp = VEC_DOT_4(a,a);\ - GIM_INV_SQRT(_pp,l);\ -}\ - - +#define VEC_INV_LENGTH_4(a, l) \ + { \ + GREAL _pp = VEC_DOT_4(a, a); \ + GIM_INV_SQRT(_pp, l); \ + } /// distance between two points -#define VEC_DISTANCE(_len,_va,_vb) {\ - vec3f _tmp_; \ - VEC_DIFF(_tmp_, _vb, _va); \ - VEC_LENGTH(_tmp_,_len); \ -}\ - +#define VEC_DISTANCE(_len, _va, _vb) \ + { \ + vec3f _tmp_; \ + VEC_DIFF(_tmp_, _vb, _va); \ + VEC_LENGTH(_tmp_, _len); \ + } /// Vector length -#define VEC_CONJUGATE_LENGTH(a,l)\ -{\ - GREAL _pp = 1.0 - a[0]*a[0] - a[1]*a[1] - a[2]*a[2];\ - GIM_SQRT(_pp,l);\ -}\ - +#define VEC_CONJUGATE_LENGTH(a, l) \ + { \ + GREAL _pp = 1.0 - a[0] * a[0] - a[1] * a[1] - a[2] * a[2]; \ + GIM_SQRT(_pp, l); \ + } /// Vector length -#define VEC_NORMALIZE(a) { \ - GREAL len;\ - VEC_INV_LENGTH(a,len); \ - if(len<G_REAL_INFINITY)\ - {\ - a[0] *= len; \ - a[1] *= len; \ - a[2] *= len; \ - } \ -}\ +#define VEC_NORMALIZE(a) \ + { \ + GREAL len; \ + VEC_INV_LENGTH(a, len); \ + if (len < G_REAL_INFINITY) \ + { \ + a[0] *= len; \ + a[1] *= len; \ + a[2] *= len; \ + } \ + } /// Set Vector size -#define VEC_RENORMALIZE(a,newlen) { \ - GREAL len;\ - VEC_INV_LENGTH(a,len); \ - if(len<G_REAL_INFINITY)\ - {\ - len *= newlen;\ - a[0] *= len; \ - a[1] *= len; \ - a[2] *= len; \ - } \ -}\ +#define VEC_RENORMALIZE(a, newlen) \ + { \ + GREAL len; \ + VEC_INV_LENGTH(a, len); \ + if (len < G_REAL_INFINITY) \ + { \ + len *= newlen; \ + a[0] *= len; \ + a[1] *= len; \ + a[2] *= len; \ + } \ + } /// Vector cross -#define VEC_CROSS(c,a,b) \ -{ \ - c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1]; \ - c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2]; \ - c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0]; \ -}\ - +#define VEC_CROSS(c, a, b) \ + { \ + c[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1]; \ + c[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2]; \ + c[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0]; \ + } /*! Vector perp -- assumes that n is of unit length * accepts vector v, subtracts out any component parallel to n */ -#define VEC_PERPENDICULAR(vp,v,n) \ -{ \ - GREAL dot = VEC_DOT(v, n); \ - vp[0] = (v)[0] - dot*(n)[0]; \ - vp[1] = (v)[1] - dot*(n)[1]; \ - vp[2] = (v)[2] - dot*(n)[2]; \ -}\ - +#define VEC_PERPENDICULAR(vp, v, n) \ + { \ + GREAL dot = VEC_DOT(v, n); \ + vp[0] = (v)[0] - dot * (n)[0]; \ + vp[1] = (v)[1] - dot * (n)[1]; \ + vp[2] = (v)[2] - dot * (n)[2]; \ + } /*! Vector parallel -- assumes that n is of unit length */ -#define VEC_PARALLEL(vp,v,n) \ -{ \ - GREAL dot = VEC_DOT(v, n); \ - vp[0] = (dot) * (n)[0]; \ - vp[1] = (dot) * (n)[1]; \ - vp[2] = (dot) * (n)[2]; \ -}\ +#define VEC_PARALLEL(vp, v, n) \ + { \ + GREAL dot = VEC_DOT(v, n); \ + vp[0] = (dot) * (n)[0]; \ + vp[1] = (dot) * (n)[1]; \ + vp[2] = (dot) * (n)[2]; \ + } /*! Same as Vector parallel -- n can have any length * accepts vector v, subtracts out any component perpendicular to n */ -#define VEC_PROJECT(vp,v,n) \ -{ \ - GREAL scalar = VEC_DOT(v, n); \ - scalar/= VEC_DOT(n, n); \ - vp[0] = (scalar) * (n)[0]; \ - vp[1] = (scalar) * (n)[1]; \ - vp[2] = (scalar) * (n)[2]; \ -}\ - +#define VEC_PROJECT(vp, v, n) \ + { \ + GREAL scalar = VEC_DOT(v, n); \ + scalar /= VEC_DOT(n, n); \ + vp[0] = (scalar) * (n)[0]; \ + vp[1] = (scalar) * (n)[1]; \ + vp[2] = (scalar) * (n)[2]; \ + } /*! accepts vector v*/ -#define VEC_UNPROJECT(vp,v,n) \ -{ \ - GREAL scalar = VEC_DOT(v, n); \ - scalar = VEC_DOT(n, n)/scalar; \ - vp[0] = (scalar) * (n)[0]; \ - vp[1] = (scalar) * (n)[1]; \ - vp[2] = (scalar) * (n)[2]; \ -}\ - +#define VEC_UNPROJECT(vp, v, n) \ + { \ + GREAL scalar = VEC_DOT(v, n); \ + scalar = VEC_DOT(n, n) / scalar; \ + vp[0] = (scalar) * (n)[0]; \ + vp[1] = (scalar) * (n)[1]; \ + vp[2] = (scalar) * (n)[2]; \ + } /*! Vector reflection -- assumes n is of unit length Takes vector v, reflects it against reflector n, and returns vr */ -#define VEC_REFLECT(vr,v,n) \ -{ \ - GREAL dot = VEC_DOT(v, n); \ - vr[0] = (v)[0] - 2.0 * (dot) * (n)[0]; \ - vr[1] = (v)[1] - 2.0 * (dot) * (n)[1]; \ - vr[2] = (v)[2] - 2.0 * (dot) * (n)[2]; \ -}\ - +#define VEC_REFLECT(vr, v, n) \ + { \ + GREAL dot = VEC_DOT(v, n); \ + vr[0] = (v)[0] - 2.0 * (dot) * (n)[0]; \ + vr[1] = (v)[1] - 2.0 * (dot) * (n)[1]; \ + vr[2] = (v)[2] - 2.0 * (dot) * (n)[2]; \ + } /*! Vector blending Takes two vectors a, b, blends them together with two scalars */ -#define VEC_BLEND_AB(vr,sa,a,sb,b) \ -{ \ - vr[0] = (sa) * (a)[0] + (sb) * (b)[0]; \ - vr[1] = (sa) * (a)[1] + (sb) * (b)[1]; \ - vr[2] = (sa) * (a)[2] + (sb) * (b)[2]; \ -}\ +#define VEC_BLEND_AB(vr, sa, a, sb, b) \ + { \ + vr[0] = (sa) * (a)[0] + (sb) * (b)[0]; \ + vr[1] = (sa) * (a)[1] + (sb) * (b)[1]; \ + vr[2] = (sa) * (a)[2] + (sb) * (b)[2]; \ + } /*! Vector blending Takes two vectors a, b, blends them together with s <=1 */ -#define VEC_BLEND(vr,a,b,s) VEC_BLEND_AB(vr,(1-s),a,s,b) +#define VEC_BLEND(vr, a, b, s) VEC_BLEND_AB(vr, (1 - s), a, s, b) -#define VEC_SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2]; +#define VEC_SET3(a, b, op, c) \ + a[0] = b[0] op c[0]; \ + a[1] = b[1] op c[1]; \ + a[2] = b[2] op c[2]; //! Finds the bigger cartesian coordinate from a vector -#define VEC_MAYOR_COORD(vec, maxc)\ -{\ - GREAL A[] = {fabs(vec[0]),fabs(vec[1]),fabs(vec[2])};\ - maxc = A[0]>A[1]?(A[0]>A[2]?0:2):(A[1]>A[2]?1:2);\ -}\ +#define VEC_MAYOR_COORD(vec, maxc) \ + { \ + GREAL A[] = {fabs(vec[0]), fabs(vec[1]), fabs(vec[2])}; \ + maxc = A[0] > A[1] ? (A[0] > A[2] ? 0 : 2) : (A[1] > A[2] ? 1 : 2); \ + } //! Finds the 2 smallest cartesian coordinates from a vector -#define VEC_MINOR_AXES(vec, i0, i1)\ -{\ - VEC_MAYOR_COORD(vec,i0);\ - i0 = (i0+1)%3;\ - i1 = (i0+1)%3;\ -}\ - - - +#define VEC_MINOR_AXES(vec, i0, i1) \ + { \ + VEC_MAYOR_COORD(vec, i0); \ + i0 = (i0 + 1) % 3; \ + i1 = (i0 + 1) % 3; \ + } -#define VEC_EQUAL(v1,v2) (v1[0]==v2[0]&&v1[1]==v2[1]&&v1[2]==v2[2]) - -#define VEC_NEAR_EQUAL(v1,v2) (GIM_NEAR_EQUAL(v1[0],v2[0])&&GIM_NEAR_EQUAL(v1[1],v2[1])&&GIM_NEAR_EQUAL(v1[2],v2[2])) +#define VEC_EQUAL(v1, v2) (v1[0] == v2[0] && v1[1] == v2[1] && v1[2] == v2[2]) +#define VEC_NEAR_EQUAL(v1, v2) (GIM_NEAR_EQUAL(v1[0], v2[0]) && GIM_NEAR_EQUAL(v1[1], v2[1]) && GIM_NEAR_EQUAL(v1[2], v2[2])) /// Vector cross -#define X_AXIS_CROSS_VEC(dst,src)\ -{ \ - dst[0] = 0.0f; \ - dst[1] = -src[2]; \ - dst[2] = src[1]; \ -}\ - -#define Y_AXIS_CROSS_VEC(dst,src)\ -{ \ - dst[0] = src[2]; \ - dst[1] = 0.0f; \ - dst[2] = -src[0]; \ -}\ - -#define Z_AXIS_CROSS_VEC(dst,src)\ -{ \ - dst[0] = -src[1]; \ - dst[1] = src[0]; \ - dst[2] = 0.0f; \ -}\ - - - - - +#define X_AXIS_CROSS_VEC(dst, src) \ + { \ + dst[0] = 0.0f; \ + dst[1] = -src[2]; \ + dst[2] = src[1]; \ + } + +#define Y_AXIS_CROSS_VEC(dst, src) \ + { \ + dst[0] = src[2]; \ + dst[1] = 0.0f; \ + dst[2] = -src[0]; \ + } + +#define Z_AXIS_CROSS_VEC(dst, src) \ + { \ + dst[0] = -src[1]; \ + dst[1] = src[0]; \ + dst[2] = 0.0f; \ + } /// initialize matrix -#define IDENTIFY_MATRIX_3X3(m) \ -{ \ - m[0][0] = 1.0; \ - m[0][1] = 0.0; \ - m[0][2] = 0.0; \ - \ - m[1][0] = 0.0; \ - m[1][1] = 1.0; \ - m[1][2] = 0.0; \ - \ - m[2][0] = 0.0; \ - m[2][1] = 0.0; \ - m[2][2] = 1.0; \ -}\ +#define IDENTIFY_MATRIX_3X3(m) \ + { \ + m[0][0] = 1.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 1.0; \ + m[1][2] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 1.0; \ + } /*! initialize matrix */ -#define IDENTIFY_MATRIX_4X4(m) \ -{ \ - m[0][0] = 1.0; \ - m[0][1] = 0.0; \ - m[0][2] = 0.0; \ - m[0][3] = 0.0; \ - \ - m[1][0] = 0.0; \ - m[1][1] = 1.0; \ - m[1][2] = 0.0; \ - m[1][3] = 0.0; \ - \ - m[2][0] = 0.0; \ - m[2][1] = 0.0; \ - m[2][2] = 1.0; \ - m[2][3] = 0.0; \ - \ - m[3][0] = 0.0; \ - m[3][1] = 0.0; \ - m[3][2] = 0.0; \ - m[3][3] = 1.0; \ -}\ +#define IDENTIFY_MATRIX_4X4(m) \ + { \ + m[0][0] = 1.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 1.0; \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 1.0; \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ + } /*! initialize matrix */ -#define ZERO_MATRIX_4X4(m) \ -{ \ - m[0][0] = 0.0; \ - m[0][1] = 0.0; \ - m[0][2] = 0.0; \ - m[0][3] = 0.0; \ - \ - m[1][0] = 0.0; \ - m[1][1] = 0.0; \ - m[1][2] = 0.0; \ - m[1][3] = 0.0; \ - \ - m[2][0] = 0.0; \ - m[2][1] = 0.0; \ - m[2][2] = 0.0; \ - m[2][3] = 0.0; \ - \ - m[3][0] = 0.0; \ - m[3][1] = 0.0; \ - m[3][2] = 0.0; \ - m[3][3] = 0.0; \ -}\ +#define ZERO_MATRIX_4X4(m) \ + { \ + m[0][0] = 0.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 0.0; \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 0.0; \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 0.0; \ + } /*! matrix rotation X */ -#define ROTX_CS(m,cosine,sine) \ -{ \ - /* rotation about the x-axis */ \ - \ - m[0][0] = 1.0; \ - m[0][1] = 0.0; \ - m[0][2] = 0.0; \ - m[0][3] = 0.0; \ - \ - m[1][0] = 0.0; \ - m[1][1] = (cosine); \ - m[1][2] = (sine); \ - m[1][3] = 0.0; \ - \ - m[2][0] = 0.0; \ - m[2][1] = -(sine); \ - m[2][2] = (cosine); \ - m[2][3] = 0.0; \ - \ - m[3][0] = 0.0; \ - m[3][1] = 0.0; \ - m[3][2] = 0.0; \ - m[3][3] = 1.0; \ -}\ +#define ROTX_CS(m, cosine, sine) \ + { \ + /* rotation about the x-axis */ \ + \ + m[0][0] = 1.0; \ + m[0][1] = 0.0; \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = (cosine); \ + m[1][2] = (sine); \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = -(sine); \ + m[2][2] = (cosine); \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ + } /*! matrix rotation Y */ -#define ROTY_CS(m,cosine,sine) \ -{ \ - /* rotation about the y-axis */ \ - \ - m[0][0] = (cosine); \ - m[0][1] = 0.0; \ - m[0][2] = -(sine); \ - m[0][3] = 0.0; \ - \ - m[1][0] = 0.0; \ - m[1][1] = 1.0; \ - m[1][2] = 0.0; \ - m[1][3] = 0.0; \ - \ - m[2][0] = (sine); \ - m[2][1] = 0.0; \ - m[2][2] = (cosine); \ - m[2][3] = 0.0; \ - \ - m[3][0] = 0.0; \ - m[3][1] = 0.0; \ - m[3][2] = 0.0; \ - m[3][3] = 1.0; \ -}\ +#define ROTY_CS(m, cosine, sine) \ + { \ + /* rotation about the y-axis */ \ + \ + m[0][0] = (cosine); \ + m[0][1] = 0.0; \ + m[0][2] = -(sine); \ + m[0][3] = 0.0; \ + \ + m[1][0] = 0.0; \ + m[1][1] = 1.0; \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = (sine); \ + m[2][1] = 0.0; \ + m[2][2] = (cosine); \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ + } /*! matrix rotation Z */ -#define ROTZ_CS(m,cosine,sine) \ -{ \ - /* rotation about the z-axis */ \ - \ - m[0][0] = (cosine); \ - m[0][1] = (sine); \ - m[0][2] = 0.0; \ - m[0][3] = 0.0; \ - \ - m[1][0] = -(sine); \ - m[1][1] = (cosine); \ - m[1][2] = 0.0; \ - m[1][3] = 0.0; \ - \ - m[2][0] = 0.0; \ - m[2][1] = 0.0; \ - m[2][2] = 1.0; \ - m[2][3] = 0.0; \ - \ - m[3][0] = 0.0; \ - m[3][1] = 0.0; \ - m[3][2] = 0.0; \ - m[3][3] = 1.0; \ -}\ +#define ROTZ_CS(m, cosine, sine) \ + { \ + /* rotation about the z-axis */ \ + \ + m[0][0] = (cosine); \ + m[0][1] = (sine); \ + m[0][2] = 0.0; \ + m[0][3] = 0.0; \ + \ + m[1][0] = -(sine); \ + m[1][1] = (cosine); \ + m[1][2] = 0.0; \ + m[1][3] = 0.0; \ + \ + m[2][0] = 0.0; \ + m[2][1] = 0.0; \ + m[2][2] = 1.0; \ + m[2][3] = 0.0; \ + \ + m[3][0] = 0.0; \ + m[3][1] = 0.0; \ + m[3][2] = 0.0; \ + m[3][3] = 1.0; \ + } /*! matrix copy */ -#define COPY_MATRIX_2X2(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[0][1]; \ - \ - b[1][0] = a[1][0]; \ - b[1][1] = a[1][1]; \ - \ -}\ - +#define COPY_MATRIX_2X2(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + } /*! matrix copy */ -#define COPY_MATRIX_2X3(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[0][1]; \ - b[0][2] = a[0][2]; \ - \ - b[1][0] = a[1][0]; \ - b[1][1] = a[1][1]; \ - b[1][2] = a[1][2]; \ -}\ - +#define COPY_MATRIX_2X3(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + b[0][2] = a[0][2]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[1][2]; \ + } /*! matrix copy */ -#define COPY_MATRIX_3X3(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[0][1]; \ - b[0][2] = a[0][2]; \ - \ - b[1][0] = a[1][0]; \ - b[1][1] = a[1][1]; \ - b[1][2] = a[1][2]; \ - \ - b[2][0] = a[2][0]; \ - b[2][1] = a[2][1]; \ - b[2][2] = a[2][2]; \ -}\ - +#define COPY_MATRIX_3X3(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + b[0][2] = a[0][2]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[1][2]; \ + \ + b[2][0] = a[2][0]; \ + b[2][1] = a[2][1]; \ + b[2][2] = a[2][2]; \ + } /*! matrix copy */ -#define COPY_MATRIX_4X4(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[0][1]; \ - b[0][2] = a[0][2]; \ - b[0][3] = a[0][3]; \ - \ - b[1][0] = a[1][0]; \ - b[1][1] = a[1][1]; \ - b[1][2] = a[1][2]; \ - b[1][3] = a[1][3]; \ - \ - b[2][0] = a[2][0]; \ - b[2][1] = a[2][1]; \ - b[2][2] = a[2][2]; \ - b[2][3] = a[2][3]; \ - \ - b[3][0] = a[3][0]; \ - b[3][1] = a[3][1]; \ - b[3][2] = a[3][2]; \ - b[3][3] = a[3][3]; \ -}\ - +#define COPY_MATRIX_4X4(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[0][1]; \ + b[0][2] = a[0][2]; \ + b[0][3] = a[0][3]; \ + \ + b[1][0] = a[1][0]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[1][2]; \ + b[1][3] = a[1][3]; \ + \ + b[2][0] = a[2][0]; \ + b[2][1] = a[2][1]; \ + b[2][2] = a[2][2]; \ + b[2][3] = a[2][3]; \ + \ + b[3][0] = a[3][0]; \ + b[3][1] = a[3][1]; \ + b[3][2] = a[3][2]; \ + b[3][3] = a[3][3]; \ + } /*! matrix transpose */ -#define TRANSPOSE_MATRIX_2X2(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[1][0]; \ - \ - b[1][0] = a[0][1]; \ - b[1][1] = a[1][1]; \ -}\ - +#define TRANSPOSE_MATRIX_2X2(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[1][0]; \ + \ + b[1][0] = a[0][1]; \ + b[1][1] = a[1][1]; \ + } /*! matrix transpose */ -#define TRANSPOSE_MATRIX_3X3(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[1][0]; \ - b[0][2] = a[2][0]; \ - \ - b[1][0] = a[0][1]; \ - b[1][1] = a[1][1]; \ - b[1][2] = a[2][1]; \ - \ - b[2][0] = a[0][2]; \ - b[2][1] = a[1][2]; \ - b[2][2] = a[2][2]; \ -}\ - +#define TRANSPOSE_MATRIX_3X3(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[1][0]; \ + b[0][2] = a[2][0]; \ + \ + b[1][0] = a[0][1]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[2][1]; \ + \ + b[2][0] = a[0][2]; \ + b[2][1] = a[1][2]; \ + b[2][2] = a[2][2]; \ + } /*! matrix transpose */ -#define TRANSPOSE_MATRIX_4X4(b,a) \ -{ \ - b[0][0] = a[0][0]; \ - b[0][1] = a[1][0]; \ - b[0][2] = a[2][0]; \ - b[0][3] = a[3][0]; \ - \ - b[1][0] = a[0][1]; \ - b[1][1] = a[1][1]; \ - b[1][2] = a[2][1]; \ - b[1][3] = a[3][1]; \ - \ - b[2][0] = a[0][2]; \ - b[2][1] = a[1][2]; \ - b[2][2] = a[2][2]; \ - b[2][3] = a[3][2]; \ - \ - b[3][0] = a[0][3]; \ - b[3][1] = a[1][3]; \ - b[3][2] = a[2][3]; \ - b[3][3] = a[3][3]; \ -}\ - +#define TRANSPOSE_MATRIX_4X4(b, a) \ + { \ + b[0][0] = a[0][0]; \ + b[0][1] = a[1][0]; \ + b[0][2] = a[2][0]; \ + b[0][3] = a[3][0]; \ + \ + b[1][0] = a[0][1]; \ + b[1][1] = a[1][1]; \ + b[1][2] = a[2][1]; \ + b[1][3] = a[3][1]; \ + \ + b[2][0] = a[0][2]; \ + b[2][1] = a[1][2]; \ + b[2][2] = a[2][2]; \ + b[2][3] = a[3][2]; \ + \ + b[3][0] = a[0][3]; \ + b[3][1] = a[1][3]; \ + b[3][2] = a[2][3]; \ + b[3][3] = a[3][3]; \ + } /*! multiply matrix by scalar */ -#define SCALE_MATRIX_2X2(b,s,a) \ -{ \ - b[0][0] = (s) * a[0][0]; \ - b[0][1] = (s) * a[0][1]; \ - \ - b[1][0] = (s) * a[1][0]; \ - b[1][1] = (s) * a[1][1]; \ -}\ - +#define SCALE_MATRIX_2X2(b, s, a) \ + { \ + b[0][0] = (s)*a[0][0]; \ + b[0][1] = (s)*a[0][1]; \ + \ + b[1][0] = (s)*a[1][0]; \ + b[1][1] = (s)*a[1][1]; \ + } /*! multiply matrix by scalar */ -#define SCALE_MATRIX_3X3(b,s,a) \ -{ \ - b[0][0] = (s) * a[0][0]; \ - b[0][1] = (s) * a[0][1]; \ - b[0][2] = (s) * a[0][2]; \ - \ - b[1][0] = (s) * a[1][0]; \ - b[1][1] = (s) * a[1][1]; \ - b[1][2] = (s) * a[1][2]; \ - \ - b[2][0] = (s) * a[2][0]; \ - b[2][1] = (s) * a[2][1]; \ - b[2][2] = (s) * a[2][2]; \ -}\ - +#define SCALE_MATRIX_3X3(b, s, a) \ + { \ + b[0][0] = (s)*a[0][0]; \ + b[0][1] = (s)*a[0][1]; \ + b[0][2] = (s)*a[0][2]; \ + \ + b[1][0] = (s)*a[1][0]; \ + b[1][1] = (s)*a[1][1]; \ + b[1][2] = (s)*a[1][2]; \ + \ + b[2][0] = (s)*a[2][0]; \ + b[2][1] = (s)*a[2][1]; \ + b[2][2] = (s)*a[2][2]; \ + } /*! multiply matrix by scalar */ -#define SCALE_MATRIX_4X4(b,s,a) \ -{ \ - b[0][0] = (s) * a[0][0]; \ - b[0][1] = (s) * a[0][1]; \ - b[0][2] = (s) * a[0][2]; \ - b[0][3] = (s) * a[0][3]; \ - \ - b[1][0] = (s) * a[1][0]; \ - b[1][1] = (s) * a[1][1]; \ - b[1][2] = (s) * a[1][2]; \ - b[1][3] = (s) * a[1][3]; \ - \ - b[2][0] = (s) * a[2][0]; \ - b[2][1] = (s) * a[2][1]; \ - b[2][2] = (s) * a[2][2]; \ - b[2][3] = (s) * a[2][3]; \ - \ - b[3][0] = s * a[3][0]; \ - b[3][1] = s * a[3][1]; \ - b[3][2] = s * a[3][2]; \ - b[3][3] = s * a[3][3]; \ -}\ - +#define SCALE_MATRIX_4X4(b, s, a) \ + { \ + b[0][0] = (s)*a[0][0]; \ + b[0][1] = (s)*a[0][1]; \ + b[0][2] = (s)*a[0][2]; \ + b[0][3] = (s)*a[0][3]; \ + \ + b[1][0] = (s)*a[1][0]; \ + b[1][1] = (s)*a[1][1]; \ + b[1][2] = (s)*a[1][2]; \ + b[1][3] = (s)*a[1][3]; \ + \ + b[2][0] = (s)*a[2][0]; \ + b[2][1] = (s)*a[2][1]; \ + b[2][2] = (s)*a[2][2]; \ + b[2][3] = (s)*a[2][3]; \ + \ + b[3][0] = s * a[3][0]; \ + b[3][1] = s * a[3][1]; \ + b[3][2] = s * a[3][2]; \ + b[3][3] = s * a[3][3]; \ + } /*! multiply matrix by scalar */ -#define SCALE_VEC_MATRIX_2X2(b,svec,a) \ -{ \ - b[0][0] = svec[0] * a[0][0]; \ - b[1][0] = svec[0] * a[1][0]; \ - \ - b[0][1] = svec[1] * a[0][1]; \ - b[1][1] = svec[1] * a[1][1]; \ -}\ - +#define SCALE_VEC_MATRIX_2X2(b, svec, a) \ + { \ + b[0][0] = svec[0] * a[0][0]; \ + b[1][0] = svec[0] * a[1][0]; \ + \ + b[0][1] = svec[1] * a[0][1]; \ + b[1][1] = svec[1] * a[1][1]; \ + } /*! multiply matrix by scalar. Each columns is scaled by each scalar vector component */ -#define SCALE_VEC_MATRIX_3X3(b,svec,a) \ -{ \ - b[0][0] = svec[0] * a[0][0]; \ - b[1][0] = svec[0] * a[1][0]; \ - b[2][0] = svec[0] * a[2][0]; \ - \ - b[0][1] = svec[1] * a[0][1]; \ - b[1][1] = svec[1] * a[1][1]; \ - b[2][1] = svec[1] * a[2][1]; \ - \ - b[0][2] = svec[2] * a[0][2]; \ - b[1][2] = svec[2] * a[1][2]; \ - b[2][2] = svec[2] * a[2][2]; \ -}\ - +#define SCALE_VEC_MATRIX_3X3(b, svec, a) \ + { \ + b[0][0] = svec[0] * a[0][0]; \ + b[1][0] = svec[0] * a[1][0]; \ + b[2][0] = svec[0] * a[2][0]; \ + \ + b[0][1] = svec[1] * a[0][1]; \ + b[1][1] = svec[1] * a[1][1]; \ + b[2][1] = svec[1] * a[2][1]; \ + \ + b[0][2] = svec[2] * a[0][2]; \ + b[1][2] = svec[2] * a[1][2]; \ + b[2][2] = svec[2] * a[2][2]; \ + } /*! multiply matrix by scalar */ -#define SCALE_VEC_MATRIX_4X4(b,svec,a) \ -{ \ - b[0][0] = svec[0] * a[0][0]; \ - b[1][0] = svec[0] * a[1][0]; \ - b[2][0] = svec[0] * a[2][0]; \ - b[3][0] = svec[0] * a[3][0]; \ - \ - b[0][1] = svec[1] * a[0][1]; \ - b[1][1] = svec[1] * a[1][1]; \ - b[2][1] = svec[1] * a[2][1]; \ - b[3][1] = svec[1] * a[3][1]; \ - \ - b[0][2] = svec[2] * a[0][2]; \ - b[1][2] = svec[2] * a[1][2]; \ - b[2][2] = svec[2] * a[2][2]; \ - b[3][2] = svec[2] * a[3][2]; \ - \ - b[0][3] = svec[3] * a[0][3]; \ - b[1][3] = svec[3] * a[1][3]; \ - b[2][3] = svec[3] * a[2][3]; \ - b[3][3] = svec[3] * a[3][3]; \ -}\ - +#define SCALE_VEC_MATRIX_4X4(b, svec, a) \ + { \ + b[0][0] = svec[0] * a[0][0]; \ + b[1][0] = svec[0] * a[1][0]; \ + b[2][0] = svec[0] * a[2][0]; \ + b[3][0] = svec[0] * a[3][0]; \ + \ + b[0][1] = svec[1] * a[0][1]; \ + b[1][1] = svec[1] * a[1][1]; \ + b[2][1] = svec[1] * a[2][1]; \ + b[3][1] = svec[1] * a[3][1]; \ + \ + b[0][2] = svec[2] * a[0][2]; \ + b[1][2] = svec[2] * a[1][2]; \ + b[2][2] = svec[2] * a[2][2]; \ + b[3][2] = svec[2] * a[3][2]; \ + \ + b[0][3] = svec[3] * a[0][3]; \ + b[1][3] = svec[3] * a[1][3]; \ + b[2][3] = svec[3] * a[2][3]; \ + b[3][3] = svec[3] * a[3][3]; \ + } /*! multiply matrix by scalar */ -#define ACCUM_SCALE_MATRIX_2X2(b,s,a) \ -{ \ - b[0][0] += (s) * a[0][0]; \ - b[0][1] += (s) * a[0][1]; \ - \ - b[1][0] += (s) * a[1][0]; \ - b[1][1] += (s) * a[1][1]; \ -}\ - +#define ACCUM_SCALE_MATRIX_2X2(b, s, a) \ + { \ + b[0][0] += (s)*a[0][0]; \ + b[0][1] += (s)*a[0][1]; \ + \ + b[1][0] += (s)*a[1][0]; \ + b[1][1] += (s)*a[1][1]; \ + } /*! multiply matrix by scalar */ -#define ACCUM_SCALE_MATRIX_3X3(b,s,a) \ -{ \ - b[0][0] += (s) * a[0][0]; \ - b[0][1] += (s) * a[0][1]; \ - b[0][2] += (s) * a[0][2]; \ - \ - b[1][0] += (s) * a[1][0]; \ - b[1][1] += (s) * a[1][1]; \ - b[1][2] += (s) * a[1][2]; \ - \ - b[2][0] += (s) * a[2][0]; \ - b[2][1] += (s) * a[2][1]; \ - b[2][2] += (s) * a[2][2]; \ -}\ - +#define ACCUM_SCALE_MATRIX_3X3(b, s, a) \ + { \ + b[0][0] += (s)*a[0][0]; \ + b[0][1] += (s)*a[0][1]; \ + b[0][2] += (s)*a[0][2]; \ + \ + b[1][0] += (s)*a[1][0]; \ + b[1][1] += (s)*a[1][1]; \ + b[1][2] += (s)*a[1][2]; \ + \ + b[2][0] += (s)*a[2][0]; \ + b[2][1] += (s)*a[2][1]; \ + b[2][2] += (s)*a[2][2]; \ + } /*! multiply matrix by scalar */ -#define ACCUM_SCALE_MATRIX_4X4(b,s,a) \ -{ \ - b[0][0] += (s) * a[0][0]; \ - b[0][1] += (s) * a[0][1]; \ - b[0][2] += (s) * a[0][2]; \ - b[0][3] += (s) * a[0][3]; \ - \ - b[1][0] += (s) * a[1][0]; \ - b[1][1] += (s) * a[1][1]; \ - b[1][2] += (s) * a[1][2]; \ - b[1][3] += (s) * a[1][3]; \ - \ - b[2][0] += (s) * a[2][0]; \ - b[2][1] += (s) * a[2][1]; \ - b[2][2] += (s) * a[2][2]; \ - b[2][3] += (s) * a[2][3]; \ - \ - b[3][0] += (s) * a[3][0]; \ - b[3][1] += (s) * a[3][1]; \ - b[3][2] += (s) * a[3][2]; \ - b[3][3] += (s) * a[3][3]; \ -}\ +#define ACCUM_SCALE_MATRIX_4X4(b, s, a) \ + { \ + b[0][0] += (s)*a[0][0]; \ + b[0][1] += (s)*a[0][1]; \ + b[0][2] += (s)*a[0][2]; \ + b[0][3] += (s)*a[0][3]; \ + \ + b[1][0] += (s)*a[1][0]; \ + b[1][1] += (s)*a[1][1]; \ + b[1][2] += (s)*a[1][2]; \ + b[1][3] += (s)*a[1][3]; \ + \ + b[2][0] += (s)*a[2][0]; \ + b[2][1] += (s)*a[2][1]; \ + b[2][2] += (s)*a[2][2]; \ + b[2][3] += (s)*a[2][3]; \ + \ + b[3][0] += (s)*a[3][0]; \ + b[3][1] += (s)*a[3][1]; \ + b[3][2] += (s)*a[3][2]; \ + b[3][3] += (s)*a[3][3]; \ + } /*! matrix product */ /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ -#define MATRIX_PRODUCT_2X2(c,a,b) \ -{ \ - c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]; \ - c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]; \ - \ - c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]; \ - c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]; \ - \ -}\ +#define MATRIX_PRODUCT_2X2(c, a, b) \ + { \ + c[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0]; \ + c[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1]; \ + \ + c[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0]; \ + c[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1]; \ + } /*! matrix product */ /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ -#define MATRIX_PRODUCT_3X3(c,a,b) \ -{ \ - c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]; \ - c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]; \ - c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]; \ - \ - c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]; \ - c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]; \ - c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]; \ - \ - c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]; \ - c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]; \ - c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]; \ -}\ - +#define MATRIX_PRODUCT_3X3(c, a, b) \ + { \ + c[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0]; \ + c[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1]; \ + c[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2]; \ + \ + c[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0]; \ + c[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1]; \ + c[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2]; \ + \ + c[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0]; \ + c[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1]; \ + c[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2]; \ + } /*! matrix product */ /*! c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y];*/ -#define MATRIX_PRODUCT_4X4(c,a,b) \ -{ \ - c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];\ - c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];\ - c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];\ - c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];\ - \ - c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];\ - c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];\ - c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];\ - c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];\ - \ - c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];\ - c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];\ - c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];\ - c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];\ - \ - c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];\ - c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];\ - c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];\ - c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];\ -}\ - +#define MATRIX_PRODUCT_4X4(c, a, b) \ + { \ + c[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0] + a[0][3] * b[3][0]; \ + c[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1] + a[0][3] * b[3][1]; \ + c[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2] + a[0][3] * b[3][2]; \ + c[0][3] = a[0][0] * b[0][3] + a[0][1] * b[1][3] + a[0][2] * b[2][3] + a[0][3] * b[3][3]; \ + \ + c[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0] + a[1][3] * b[3][0]; \ + c[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1] + a[1][3] * b[3][1]; \ + c[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2] + a[1][3] * b[3][2]; \ + c[1][3] = a[1][0] * b[0][3] + a[1][1] * b[1][3] + a[1][2] * b[2][3] + a[1][3] * b[3][3]; \ + \ + c[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0] + a[2][3] * b[3][0]; \ + c[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1] + a[2][3] * b[3][1]; \ + c[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2] + a[2][3] * b[3][2]; \ + c[2][3] = a[2][0] * b[0][3] + a[2][1] * b[1][3] + a[2][2] * b[2][3] + a[2][3] * b[3][3]; \ + \ + c[3][0] = a[3][0] * b[0][0] + a[3][1] * b[1][0] + a[3][2] * b[2][0] + a[3][3] * b[3][0]; \ + c[3][1] = a[3][0] * b[0][1] + a[3][1] * b[1][1] + a[3][2] * b[2][1] + a[3][3] * b[3][1]; \ + c[3][2] = a[3][0] * b[0][2] + a[3][1] * b[1][2] + a[3][2] * b[2][2] + a[3][3] * b[3][2]; \ + c[3][3] = a[3][0] * b[0][3] + a[3][1] * b[1][3] + a[3][2] * b[2][3] + a[3][3] * b[3][3]; \ + } /*! matrix times vector */ -#define MAT_DOT_VEC_2X2(p,m,v) \ -{ \ - p[0] = m[0][0]*v[0] + m[0][1]*v[1]; \ - p[1] = m[1][0]*v[0] + m[1][1]*v[1]; \ -}\ - +#define MAT_DOT_VEC_2X2(p, m, v) \ + { \ + p[0] = m[0][0] * v[0] + m[0][1] * v[1]; \ + p[1] = m[1][0] * v[0] + m[1][1] * v[1]; \ + } /*! matrix times vector */ -#define MAT_DOT_VEC_3X3(p,m,v) \ -{ \ - p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2]; \ - p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2]; \ - p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]; \ -}\ - +#define MAT_DOT_VEC_3X3(p, m, v) \ + { \ + p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2]; \ + p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2]; \ + p[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2]; \ + } /*! matrix times vector v is a vec4f */ -#define MAT_DOT_VEC_4X4(p,m,v) \ -{ \ - p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3]; \ - p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3]; \ - p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3]; \ - p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3]; \ -}\ +#define MAT_DOT_VEC_4X4(p, m, v) \ + { \ + p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3] * v[3]; \ + p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3] * v[3]; \ + p[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3] * v[3]; \ + p[3] = m[3][0] * v[0] + m[3][1] * v[1] + m[3][2] * v[2] + m[3][3] * v[3]; \ + } /*! matrix times vector v is a vec3f and m is a mat4f<br> Last column is added as the position */ -#define MAT_DOT_VEC_3X4(p,m,v) \ -{ \ - p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]; \ - p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]; \ - p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]; \ -}\ - +#define MAT_DOT_VEC_3X4(p, m, v) \ + { \ + p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3]; \ + p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3]; \ + p[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3]; \ + } /*! vector transpose times matrix */ /*! p[j] = v[0]*m[0][j] + v[1]*m[1][j] + v[2]*m[2][j]; */ -#define VEC_DOT_MAT_3X3(p,v,m) \ -{ \ - p[0] = v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0]; \ - p[1] = v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1]; \ - p[2] = v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2]; \ -}\ - +#define VEC_DOT_MAT_3X3(p, v, m) \ + { \ + p[0] = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0]; \ + p[1] = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1]; \ + p[2] = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2]; \ + } /*! affine matrix times vector */ /** The matrix is assumed to be an affine matrix, with last two * entries representing a translation */ -#define MAT_DOT_VEC_2X3(p,m,v) \ -{ \ - p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]; \ - p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]; \ -}\ +#define MAT_DOT_VEC_2X3(p, m, v) \ + { \ + p[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2]; \ + p[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2]; \ + } //! Transform a plane -#define MAT_TRANSFORM_PLANE_4X4(pout,m,plane)\ -{ \ - pout[0] = m[0][0]*plane[0] + m[0][1]*plane[1] + m[0][2]*plane[2];\ - pout[1] = m[1][0]*plane[0] + m[1][1]*plane[1] + m[1][2]*plane[2];\ - pout[2] = m[2][0]*plane[0] + m[2][1]*plane[1] + m[2][2]*plane[2];\ - pout[3] = m[0][3]*pout[0] + m[1][3]*pout[1] + m[2][3]*pout[2] + plane[3];\ -}\ - - +#define MAT_TRANSFORM_PLANE_4X4(pout, m, plane) \ + { \ + pout[0] = m[0][0] * plane[0] + m[0][1] * plane[1] + m[0][2] * plane[2]; \ + pout[1] = m[1][0] * plane[0] + m[1][1] * plane[1] + m[1][2] * plane[2]; \ + pout[2] = m[2][0] * plane[0] + m[2][1] * plane[1] + m[2][2] * plane[2]; \ + pout[3] = m[0][3] * pout[0] + m[1][3] * pout[1] + m[2][3] * pout[2] + plane[3]; \ + } /** inverse transpose of matrix times vector * @@ -1000,22 +938,22 @@ Last column is added as the position * It will leave normals the wrong length !!! * See macro below for use on normals. */ -#define INV_TRANSP_MAT_DOT_VEC_2X2(p,m,v) \ -{ \ - GREAL det; \ - \ - det = m[0][0]*m[1][1] - m[0][1]*m[1][0]; \ - p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ - p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ - \ - /* if matrix not singular, and not orthonormal, then renormalize */ \ - if ((det!=1.0f) && (det != 0.0f)) { \ - det = 1.0f / det; \ - p[0] *= det; \ - p[1] *= det; \ - } \ -}\ - +#define INV_TRANSP_MAT_DOT_VEC_2X2(p, m, v) \ + { \ + GREAL det; \ + \ + det = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \ + p[0] = m[1][1] * v[0] - m[1][0] * v[1]; \ + p[1] = -m[0][1] * v[0] + m[0][0] * v[1]; \ + \ + /* if matrix not singular, and not orthonormal, then renormalize */ \ + if ((det != 1.0f) && (det != 0.0f)) \ + { \ + det = 1.0f / det; \ + p[0] *= det; \ + p[1] *= det; \ + } \ + } /** transform normal vector by inverse transpose of matrix * and then renormalize the vector @@ -1024,550 +962,527 @@ Last column is added as the position * and multiplies vector v into it, to yeild vector p * Vector p is then normalized. */ -#define NORM_XFORM_2X2(p,m,v) \ -{ \ - GREAL len; \ - \ - /* do nothing if off-diagonals are zero and diagonals are \ - * equal */ \ - if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) { \ - p[0] = m[1][1]*v[0] - m[1][0]*v[1]; \ - p[1] = - m[0][1]*v[0] + m[0][0]*v[1]; \ - \ - len = p[0]*p[0] + p[1]*p[1]; \ - GIM_INV_SQRT(len,len); \ - p[0] *= len; \ - p[1] *= len; \ - } else { \ - VEC_COPY_2 (p, v); \ - } \ -}\ - +#define NORM_XFORM_2X2(p, m, v) \ + { \ + GREAL len; \ + \ + /* do nothing if off-diagonals are zero and diagonals are \ + * equal */ \ + if ((m[0][1] != 0.0) || (m[1][0] != 0.0) || (m[0][0] != m[1][1])) \ + { \ + p[0] = m[1][1] * v[0] - m[1][0] * v[1]; \ + p[1] = -m[0][1] * v[0] + m[0][0] * v[1]; \ + \ + len = p[0] * p[0] + p[1] * p[1]; \ + GIM_INV_SQRT(len, len); \ + p[0] *= len; \ + p[1] *= len; \ + } \ + else \ + { \ + VEC_COPY_2(p, v); \ + } \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define OUTER_PRODUCT_2X2(m,v,t) \ -{ \ - m[0][0] = v[0] * t[0]; \ - m[0][1] = v[0] * t[1]; \ - \ - m[1][0] = v[1] * t[0]; \ - m[1][1] = v[1] * t[1]; \ -}\ - +#define OUTER_PRODUCT_2X2(m, v, t) \ + { \ + m[0][0] = v[0] * t[0]; \ + m[0][1] = v[0] * t[1]; \ + \ + m[1][0] = v[1] * t[0]; \ + m[1][1] = v[1] * t[1]; \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define OUTER_PRODUCT_3X3(m,v,t) \ -{ \ - m[0][0] = v[0] * t[0]; \ - m[0][1] = v[0] * t[1]; \ - m[0][2] = v[0] * t[2]; \ - \ - m[1][0] = v[1] * t[0]; \ - m[1][1] = v[1] * t[1]; \ - m[1][2] = v[1] * t[2]; \ - \ - m[2][0] = v[2] * t[0]; \ - m[2][1] = v[2] * t[1]; \ - m[2][2] = v[2] * t[2]; \ -}\ - +#define OUTER_PRODUCT_3X3(m, v, t) \ + { \ + m[0][0] = v[0] * t[0]; \ + m[0][1] = v[0] * t[1]; \ + m[0][2] = v[0] * t[2]; \ + \ + m[1][0] = v[1] * t[0]; \ + m[1][1] = v[1] * t[1]; \ + m[1][2] = v[1] * t[2]; \ + \ + m[2][0] = v[2] * t[0]; \ + m[2][1] = v[2] * t[1]; \ + m[2][2] = v[2] * t[2]; \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define OUTER_PRODUCT_4X4(m,v,t) \ -{ \ - m[0][0] = v[0] * t[0]; \ - m[0][1] = v[0] * t[1]; \ - m[0][2] = v[0] * t[2]; \ - m[0][3] = v[0] * t[3]; \ - \ - m[1][0] = v[1] * t[0]; \ - m[1][1] = v[1] * t[1]; \ - m[1][2] = v[1] * t[2]; \ - m[1][3] = v[1] * t[3]; \ - \ - m[2][0] = v[2] * t[0]; \ - m[2][1] = v[2] * t[1]; \ - m[2][2] = v[2] * t[2]; \ - m[2][3] = v[2] * t[3]; \ - \ - m[3][0] = v[3] * t[0]; \ - m[3][1] = v[3] * t[1]; \ - m[3][2] = v[3] * t[2]; \ - m[3][3] = v[3] * t[3]; \ -}\ - +#define OUTER_PRODUCT_4X4(m, v, t) \ + { \ + m[0][0] = v[0] * t[0]; \ + m[0][1] = v[0] * t[1]; \ + m[0][2] = v[0] * t[2]; \ + m[0][3] = v[0] * t[3]; \ + \ + m[1][0] = v[1] * t[0]; \ + m[1][1] = v[1] * t[1]; \ + m[1][2] = v[1] * t[2]; \ + m[1][3] = v[1] * t[3]; \ + \ + m[2][0] = v[2] * t[0]; \ + m[2][1] = v[2] * t[1]; \ + m[2][2] = v[2] * t[2]; \ + m[2][3] = v[2] * t[3]; \ + \ + m[3][0] = v[3] * t[0]; \ + m[3][1] = v[3] * t[1]; \ + m[3][2] = v[3] * t[2]; \ + m[3][3] = v[3] * t[3]; \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define ACCUM_OUTER_PRODUCT_2X2(m,v,t) \ -{ \ - m[0][0] += v[0] * t[0]; \ - m[0][1] += v[0] * t[1]; \ - \ - m[1][0] += v[1] * t[0]; \ - m[1][1] += v[1] * t[1]; \ -}\ - +#define ACCUM_OUTER_PRODUCT_2X2(m, v, t) \ + { \ + m[0][0] += v[0] * t[0]; \ + m[0][1] += v[0] * t[1]; \ + \ + m[1][0] += v[1] * t[0]; \ + m[1][1] += v[1] * t[1]; \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define ACCUM_OUTER_PRODUCT_3X3(m,v,t) \ -{ \ - m[0][0] += v[0] * t[0]; \ - m[0][1] += v[0] * t[1]; \ - m[0][2] += v[0] * t[2]; \ - \ - m[1][0] += v[1] * t[0]; \ - m[1][1] += v[1] * t[1]; \ - m[1][2] += v[1] * t[2]; \ - \ - m[2][0] += v[2] * t[0]; \ - m[2][1] += v[2] * t[1]; \ - m[2][2] += v[2] * t[2]; \ -}\ - +#define ACCUM_OUTER_PRODUCT_3X3(m, v, t) \ + { \ + m[0][0] += v[0] * t[0]; \ + m[0][1] += v[0] * t[1]; \ + m[0][2] += v[0] * t[2]; \ + \ + m[1][0] += v[1] * t[0]; \ + m[1][1] += v[1] * t[1]; \ + m[1][2] += v[1] * t[2]; \ + \ + m[2][0] += v[2] * t[0]; \ + m[2][1] += v[2] * t[1]; \ + m[2][2] += v[2] * t[2]; \ + } /** outer product of vector times vector transpose * * The outer product of vector v and vector transpose t yeilds * dyadic matrix m. */ -#define ACCUM_OUTER_PRODUCT_4X4(m,v,t) \ -{ \ - m[0][0] += v[0] * t[0]; \ - m[0][1] += v[0] * t[1]; \ - m[0][2] += v[0] * t[2]; \ - m[0][3] += v[0] * t[3]; \ - \ - m[1][0] += v[1] * t[0]; \ - m[1][1] += v[1] * t[1]; \ - m[1][2] += v[1] * t[2]; \ - m[1][3] += v[1] * t[3]; \ - \ - m[2][0] += v[2] * t[0]; \ - m[2][1] += v[2] * t[1]; \ - m[2][2] += v[2] * t[2]; \ - m[2][3] += v[2] * t[3]; \ - \ - m[3][0] += v[3] * t[0]; \ - m[3][1] += v[3] * t[1]; \ - m[3][2] += v[3] * t[2]; \ - m[3][3] += v[3] * t[3]; \ -}\ - +#define ACCUM_OUTER_PRODUCT_4X4(m, v, t) \ + { \ + m[0][0] += v[0] * t[0]; \ + m[0][1] += v[0] * t[1]; \ + m[0][2] += v[0] * t[2]; \ + m[0][3] += v[0] * t[3]; \ + \ + m[1][0] += v[1] * t[0]; \ + m[1][1] += v[1] * t[1]; \ + m[1][2] += v[1] * t[2]; \ + m[1][3] += v[1] * t[3]; \ + \ + m[2][0] += v[2] * t[0]; \ + m[2][1] += v[2] * t[1]; \ + m[2][2] += v[2] * t[2]; \ + m[2][3] += v[2] * t[3]; \ + \ + m[3][0] += v[3] * t[0]; \ + m[3][1] += v[3] * t[1]; \ + m[3][2] += v[3] * t[2]; \ + m[3][3] += v[3] * t[3]; \ + } /** determinant of matrix * * Computes determinant of matrix m, returning d */ -#define DETERMINANT_2X2(d,m) \ -{ \ - d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \ -}\ - +#define DETERMINANT_2X2(d, m) \ + { \ + d = m[0][0] * m[1][1] - m[0][1] * m[1][0]; \ + } /** determinant of matrix * * Computes determinant of matrix m, returning d */ -#define DETERMINANT_3X3(d,m) \ -{ \ - d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]); \ - d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]); \ - d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]); \ -}\ - +#define DETERMINANT_3X3(d, m) \ + { \ + d = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); \ + d -= m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]); \ + d += m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \ + } /** i,j,th cofactor of a 4x4 matrix * */ -#define COFACTOR_4X4_IJ(fac,m,i,j) \ -{ \ - GUINT __ii[4], __jj[4], __k; \ - \ - for (__k=0; __k<i; __k++) __ii[__k] = __k; \ - for (__k=i; __k<3; __k++) __ii[__k] = __k+1; \ - for (__k=0; __k<j; __k++) __jj[__k] = __k; \ - for (__k=j; __k<3; __k++) __jj[__k] = __k+1; \ - \ - (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[2]] \ - - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[1]]); \ - (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[2]] \ - - m[__ii[1]][__jj[2]]*m[__ii[2]][__jj[0]]);\ - (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]]*m[__ii[2]][__jj[1]] \ - - m[__ii[1]][__jj[1]]*m[__ii[2]][__jj[0]]);\ - \ - __k = i+j; \ - if ( __k != (__k/2)*2) { \ - (fac) = -(fac); \ - } \ -}\ - +#define COFACTOR_4X4_IJ(fac, m, i, j) \ + { \ + GUINT __ii[4], __jj[4], __k; \ + \ + for (__k = 0; __k < i; __k++) __ii[__k] = __k; \ + for (__k = i; __k < 3; __k++) __ii[__k] = __k + 1; \ + for (__k = 0; __k < j; __k++) __jj[__k] = __k; \ + for (__k = j; __k < 3; __k++) __jj[__k] = __k + 1; \ + \ + (fac) = m[__ii[0]][__jj[0]] * (m[__ii[1]][__jj[1]] * m[__ii[2]][__jj[2]] - m[__ii[1]][__jj[2]] * m[__ii[2]][__jj[1]]); \ + (fac) -= m[__ii[0]][__jj[1]] * (m[__ii[1]][__jj[0]] * m[__ii[2]][__jj[2]] - m[__ii[1]][__jj[2]] * m[__ii[2]][__jj[0]]); \ + (fac) += m[__ii[0]][__jj[2]] * (m[__ii[1]][__jj[0]] * m[__ii[2]][__jj[1]] - m[__ii[1]][__jj[1]] * m[__ii[2]][__jj[0]]); \ + \ + __k = i + j; \ + if (__k != (__k / 2) * 2) \ + { \ + (fac) = -(fac); \ + } \ + } /** determinant of matrix * * Computes determinant of matrix m, returning d */ -#define DETERMINANT_4X4(d,m) \ -{ \ - GREAL cofac; \ - COFACTOR_4X4_IJ (cofac, m, 0, 0); \ - d = m[0][0] * cofac; \ - COFACTOR_4X4_IJ (cofac, m, 0, 1); \ - d += m[0][1] * cofac; \ - COFACTOR_4X4_IJ (cofac, m, 0, 2); \ - d += m[0][2] * cofac; \ - COFACTOR_4X4_IJ (cofac, m, 0, 3); \ - d += m[0][3] * cofac; \ -}\ - +#define DETERMINANT_4X4(d, m) \ + { \ + GREAL cofac; \ + COFACTOR_4X4_IJ(cofac, m, 0, 0); \ + d = m[0][0] * cofac; \ + COFACTOR_4X4_IJ(cofac, m, 0, 1); \ + d += m[0][1] * cofac; \ + COFACTOR_4X4_IJ(cofac, m, 0, 2); \ + d += m[0][2] * cofac; \ + COFACTOR_4X4_IJ(cofac, m, 0, 3); \ + d += m[0][3] * cofac; \ + } /** cofactor of matrix * * Computes cofactor of matrix m, returning a */ -#define COFACTOR_2X2(a,m) \ -{ \ - a[0][0] = (m)[1][1]; \ - a[0][1] = - (m)[1][0]; \ - a[1][0] = - (m)[0][1]; \ - a[1][1] = (m)[0][0]; \ -}\ - +#define COFACTOR_2X2(a, m) \ + { \ + a[0][0] = (m)[1][1]; \ + a[0][1] = -(m)[1][0]; \ + a[1][0] = -(m)[0][1]; \ + a[1][1] = (m)[0][0]; \ + } /** cofactor of matrix * * Computes cofactor of matrix m, returning a */ -#define COFACTOR_3X3(a,m) \ -{ \ - a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \ - a[0][1] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \ - a[0][2] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \ - a[1][0] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \ - a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \ - a[1][2] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \ - a[2][0] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \ - a[2][1] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \ - a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ -}\ - +#define COFACTOR_3X3(a, m) \ + { \ + a[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1]; \ + a[0][1] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]); \ + a[0][2] = m[1][0] * m[2][1] - m[1][1] * m[2][0]; \ + a[1][0] = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]); \ + a[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0]; \ + a[1][2] = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]); \ + a[2][0] = m[0][1] * m[1][2] - m[0][2] * m[1][1]; \ + a[2][1] = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]); \ + a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ + } /** cofactor of matrix * * Computes cofactor of matrix m, returning a */ -#define COFACTOR_4X4(a,m) \ -{ \ - int i,j; \ - \ - for (i=0; i<4; i++) { \ - for (j=0; j<4; j++) { \ - COFACTOR_4X4_IJ (a[i][j], m, i, j); \ - } \ - } \ -}\ - +#define COFACTOR_4X4(a, m) \ + { \ + int i, j; \ + \ + for (i = 0; i < 4; i++) \ + { \ + for (j = 0; j < 4; j++) \ + { \ + COFACTOR_4X4_IJ(a[i][j], m, i, j); \ + } \ + } \ + } /** adjoint of matrix * * Computes adjoint of matrix m, returning a * (Note that adjoint is just the transpose of the cofactor matrix) */ -#define ADJOINT_2X2(a,m) \ -{ \ - a[0][0] = (m)[1][1]; \ - a[1][0] = - (m)[1][0]; \ - a[0][1] = - (m)[0][1]; \ - a[1][1] = (m)[0][0]; \ -}\ - +#define ADJOINT_2X2(a, m) \ + { \ + a[0][0] = (m)[1][1]; \ + a[1][0] = -(m)[1][0]; \ + a[0][1] = -(m)[0][1]; \ + a[1][1] = (m)[0][0]; \ + } /** adjoint of matrix * * Computes adjoint of matrix m, returning a * (Note that adjoint is just the transpose of the cofactor matrix) */ -#define ADJOINT_3X3(a,m) \ -{ \ - a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1]; \ - a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]); \ - a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0]; \ - a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]); \ - a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0]; \ - a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]); \ - a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1]; \ - a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]); \ - a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ -}\ - +#define ADJOINT_3X3(a, m) \ + { \ + a[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1]; \ + a[1][0] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]); \ + a[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0]; \ + a[0][1] = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]); \ + a[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0]; \ + a[2][1] = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]); \ + a[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1]; \ + a[1][2] = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]); \ + a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0]); \ + } /** adjoint of matrix * * Computes adjoint of matrix m, returning a * (Note that adjoint is just the transpose of the cofactor matrix) */ -#define ADJOINT_4X4(a,m) \ -{ \ - char _i_,_j_; \ - \ - for (_i_=0; _i_<4; _i_++) { \ - for (_j_=0; _j_<4; _j_++) { \ - COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \ - } \ - } \ -}\ - +#define ADJOINT_4X4(a, m) \ + { \ + char _i_, _j_; \ + \ + for (_i_ = 0; _i_ < 4; _i_++) \ + { \ + for (_j_ = 0; _j_ < 4; _j_++) \ + { \ + COFACTOR_4X4_IJ(a[_j_][_i_], m, _i_, _j_); \ + } \ + } \ + } /** compute adjoint of matrix and scale * * Computes adjoint of matrix m, scales it by s, returning a */ -#define SCALE_ADJOINT_2X2(a,s,m) \ -{ \ - a[0][0] = (s) * m[1][1]; \ - a[1][0] = - (s) * m[1][0]; \ - a[0][1] = - (s) * m[0][1]; \ - a[1][1] = (s) * m[0][0]; \ -}\ - +#define SCALE_ADJOINT_2X2(a, s, m) \ + { \ + a[0][0] = (s)*m[1][1]; \ + a[1][0] = -(s)*m[1][0]; \ + a[0][1] = -(s)*m[0][1]; \ + a[1][1] = (s)*m[0][0]; \ + } /** compute adjoint of matrix and scale * * Computes adjoint of matrix m, scales it by s, returning a */ -#define SCALE_ADJOINT_3X3(a,s,m) \ -{ \ - a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); \ - a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); \ - a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \ - \ - a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]); \ - a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]); \ - a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]); \ - \ - a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]); \ - a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]); \ - a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]); \ -}\ - +#define SCALE_ADJOINT_3X3(a, s, m) \ + { \ + a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); \ + a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); \ + a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); \ + \ + a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]); \ + a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]); \ + a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]); \ + \ + a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]); \ + a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]); \ + a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]); \ + } /** compute adjoint of matrix and scale * * Computes adjoint of matrix m, scales it by s, returning a */ -#define SCALE_ADJOINT_4X4(a,s,m) \ -{ \ - char _i_,_j_; \ - for (_i_=0; _i_<4; _i_++) { \ - for (_j_=0; _j_<4; _j_++) { \ - COFACTOR_4X4_IJ (a[_j_][_i_], m, _i_, _j_); \ - a[_j_][_i_] *= s; \ - } \ - } \ -}\ +#define SCALE_ADJOINT_4X4(a, s, m) \ + { \ + char _i_, _j_; \ + for (_i_ = 0; _i_ < 4; _i_++) \ + { \ + for (_j_ = 0; _j_ < 4; _j_++) \ + { \ + COFACTOR_4X4_IJ(a[_j_][_i_], m, _i_, _j_); \ + a[_j_][_i_] *= s; \ + } \ + } \ + } /** inverse of matrix * * Compute inverse of matrix a, returning determinant m and * inverse b */ -#define INVERT_2X2(b,det,a) \ -{ \ - GREAL _tmp_; \ - DETERMINANT_2X2 (det, a); \ - _tmp_ = 1.0 / (det); \ - SCALE_ADJOINT_2X2 (b, _tmp_, a); \ -}\ - +#define INVERT_2X2(b, det, a) \ + { \ + GREAL _tmp_; \ + DETERMINANT_2X2(det, a); \ + _tmp_ = 1.0 / (det); \ + SCALE_ADJOINT_2X2(b, _tmp_, a); \ + } /** inverse of matrix * * Compute inverse of matrix a, returning determinant m and * inverse b */ -#define INVERT_3X3(b,det,a) \ -{ \ - GREAL _tmp_; \ - DETERMINANT_3X3 (det, a); \ - _tmp_ = 1.0 / (det); \ - SCALE_ADJOINT_3X3 (b, _tmp_, a); \ -}\ - +#define INVERT_3X3(b, det, a) \ + { \ + GREAL _tmp_; \ + DETERMINANT_3X3(det, a); \ + _tmp_ = 1.0 / (det); \ + SCALE_ADJOINT_3X3(b, _tmp_, a); \ + } /** inverse of matrix * * Compute inverse of matrix a, returning determinant m and * inverse b */ -#define INVERT_4X4(b,det,a) \ -{ \ - GREAL _tmp_; \ - DETERMINANT_4X4 (det, a); \ - _tmp_ = 1.0 / (det); \ - SCALE_ADJOINT_4X4 (b, _tmp_, a); \ -}\ +#define INVERT_4X4(b, det, a) \ + { \ + GREAL _tmp_; \ + DETERMINANT_4X4(det, a); \ + _tmp_ = 1.0 / (det); \ + SCALE_ADJOINT_4X4(b, _tmp_, a); \ + } //! Get the triple(3) row of a transform matrix -#define MAT_GET_ROW(mat,vec3,rowindex)\ -{\ - vec3[0] = mat[rowindex][0];\ - vec3[1] = mat[rowindex][1];\ - vec3[2] = mat[rowindex][2]; \ -}\ +#define MAT_GET_ROW(mat, vec3, rowindex) \ + { \ + vec3[0] = mat[rowindex][0]; \ + vec3[1] = mat[rowindex][1]; \ + vec3[2] = mat[rowindex][2]; \ + } //! Get the tuple(2) row of a transform matrix -#define MAT_GET_ROW2(mat,vec2,rowindex)\ -{\ - vec2[0] = mat[rowindex][0];\ - vec2[1] = mat[rowindex][1];\ -}\ - +#define MAT_GET_ROW2(mat, vec2, rowindex) \ + { \ + vec2[0] = mat[rowindex][0]; \ + vec2[1] = mat[rowindex][1]; \ + } //! Get the quad (4) row of a transform matrix -#define MAT_GET_ROW4(mat,vec4,rowindex)\ -{\ - vec4[0] = mat[rowindex][0];\ - vec4[1] = mat[rowindex][1];\ - vec4[2] = mat[rowindex][2];\ - vec4[3] = mat[rowindex][3];\ -}\ +#define MAT_GET_ROW4(mat, vec4, rowindex) \ + { \ + vec4[0] = mat[rowindex][0]; \ + vec4[1] = mat[rowindex][1]; \ + vec4[2] = mat[rowindex][2]; \ + vec4[3] = mat[rowindex][3]; \ + } //! Get the triple(3) col of a transform matrix -#define MAT_GET_COL(mat,vec3,colindex)\ -{\ - vec3[0] = mat[0][colindex];\ - vec3[1] = mat[1][colindex];\ - vec3[2] = mat[2][colindex]; \ -}\ +#define MAT_GET_COL(mat, vec3, colindex) \ + { \ + vec3[0] = mat[0][colindex]; \ + vec3[1] = mat[1][colindex]; \ + vec3[2] = mat[2][colindex]; \ + } //! Get the tuple(2) col of a transform matrix -#define MAT_GET_COL2(mat,vec2,colindex)\ -{\ - vec2[0] = mat[0][colindex];\ - vec2[1] = mat[1][colindex];\ -}\ - +#define MAT_GET_COL2(mat, vec2, colindex) \ + { \ + vec2[0] = mat[0][colindex]; \ + vec2[1] = mat[1][colindex]; \ + } //! Get the quad (4) col of a transform matrix -#define MAT_GET_COL4(mat,vec4,colindex)\ -{\ - vec4[0] = mat[0][colindex];\ - vec4[1] = mat[1][colindex];\ - vec4[2] = mat[2][colindex];\ - vec4[3] = mat[3][colindex];\ -}\ +#define MAT_GET_COL4(mat, vec4, colindex) \ + { \ + vec4[0] = mat[0][colindex]; \ + vec4[1] = mat[1][colindex]; \ + vec4[2] = mat[2][colindex]; \ + vec4[3] = mat[3][colindex]; \ + } //! Get the triple(3) col of a transform matrix -#define MAT_GET_X(mat,vec3)\ -{\ - MAT_GET_COL(mat,vec3,0);\ -}\ +#define MAT_GET_X(mat, vec3) \ + { \ + MAT_GET_COL(mat, vec3, 0); \ + } //! Get the triple(3) col of a transform matrix -#define MAT_GET_Y(mat,vec3)\ -{\ - MAT_GET_COL(mat,vec3,1);\ -}\ +#define MAT_GET_Y(mat, vec3) \ + { \ + MAT_GET_COL(mat, vec3, 1); \ + } //! Get the triple(3) col of a transform matrix -#define MAT_GET_Z(mat,vec3)\ -{\ - MAT_GET_COL(mat,vec3,2);\ -}\ - +#define MAT_GET_Z(mat, vec3) \ + { \ + MAT_GET_COL(mat, vec3, 2); \ + } //! Get the triple(3) col of a transform matrix -#define MAT_SET_X(mat,vec3)\ -{\ - mat[0][0] = vec3[0];\ - mat[1][0] = vec3[1];\ - mat[2][0] = vec3[2];\ -}\ +#define MAT_SET_X(mat, vec3) \ + { \ + mat[0][0] = vec3[0]; \ + mat[1][0] = vec3[1]; \ + mat[2][0] = vec3[2]; \ + } //! Get the triple(3) col of a transform matrix -#define MAT_SET_Y(mat,vec3)\ -{\ - mat[0][1] = vec3[0];\ - mat[1][1] = vec3[1];\ - mat[2][1] = vec3[2];\ -}\ +#define MAT_SET_Y(mat, vec3) \ + { \ + mat[0][1] = vec3[0]; \ + mat[1][1] = vec3[1]; \ + mat[2][1] = vec3[2]; \ + } //! Get the triple(3) col of a transform matrix -#define MAT_SET_Z(mat,vec3)\ -{\ - mat[0][2] = vec3[0];\ - mat[1][2] = vec3[1];\ - mat[2][2] = vec3[2];\ -}\ - +#define MAT_SET_Z(mat, vec3) \ + { \ + mat[0][2] = vec3[0]; \ + mat[1][2] = vec3[1]; \ + mat[2][2] = vec3[2]; \ + } //! Get the triple(3) col of a transform matrix -#define MAT_GET_TRANSLATION(mat,vec3)\ -{\ - vec3[0] = mat[0][3];\ - vec3[1] = mat[1][3];\ - vec3[2] = mat[2][3]; \ -}\ +#define MAT_GET_TRANSLATION(mat, vec3) \ + { \ + vec3[0] = mat[0][3]; \ + vec3[1] = mat[1][3]; \ + vec3[2] = mat[2][3]; \ + } //! Set the triple(3) col of a transform matrix -#define MAT_SET_TRANSLATION(mat,vec3)\ -{\ - mat[0][3] = vec3[0];\ - mat[1][3] = vec3[1];\ - mat[2][3] = vec3[2]; \ -}\ - - +#define MAT_SET_TRANSLATION(mat, vec3) \ + { \ + mat[0][3] = vec3[0]; \ + mat[1][3] = vec3[1]; \ + mat[2][3] = vec3[2]; \ + } //! Returns the dot product between a vec3f and the row of a matrix -#define MAT_DOT_ROW(mat,vec3,rowindex) (vec3[0]*mat[rowindex][0] + vec3[1]*mat[rowindex][1] + vec3[2]*mat[rowindex][2]) +#define MAT_DOT_ROW(mat, vec3, rowindex) (vec3[0] * mat[rowindex][0] + vec3[1] * mat[rowindex][1] + vec3[2] * mat[rowindex][2]) //! Returns the dot product between a vec2f and the row of a matrix -#define MAT_DOT_ROW2(mat,vec2,rowindex) (vec2[0]*mat[rowindex][0] + vec2[1]*mat[rowindex][1]) +#define MAT_DOT_ROW2(mat, vec2, rowindex) (vec2[0] * mat[rowindex][0] + vec2[1] * mat[rowindex][1]) //! Returns the dot product between a vec4f and the row of a matrix -#define MAT_DOT_ROW4(mat,vec4,rowindex) (vec4[0]*mat[rowindex][0] + vec4[1]*mat[rowindex][1] + vec4[2]*mat[rowindex][2] + vec4[3]*mat[rowindex][3]) - +#define MAT_DOT_ROW4(mat, vec4, rowindex) (vec4[0] * mat[rowindex][0] + vec4[1] * mat[rowindex][1] + vec4[2] * mat[rowindex][2] + vec4[3] * mat[rowindex][3]) //! Returns the dot product between a vec3f and the col of a matrix -#define MAT_DOT_COL(mat,vec3,colindex) (vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex]) +#define MAT_DOT_COL(mat, vec3, colindex) (vec3[0] * mat[0][colindex] + vec3[1] * mat[1][colindex] + vec3[2] * mat[2][colindex]) //! Returns the dot product between a vec2f and the col of a matrix -#define MAT_DOT_COL2(mat,vec2,colindex) (vec2[0]*mat[0][colindex] + vec2[1]*mat[1][colindex]) +#define MAT_DOT_COL2(mat, vec2, colindex) (vec2[0] * mat[0][colindex] + vec2[1] * mat[1][colindex]) //! Returns the dot product between a vec4f and the col of a matrix -#define MAT_DOT_COL4(mat,vec4,colindex) (vec4[0]*mat[0][colindex] + vec4[1]*mat[1][colindex] + vec4[2]*mat[2][colindex] + vec4[3]*mat[3][colindex]) +#define MAT_DOT_COL4(mat, vec4, colindex) (vec4[0] * mat[0][colindex] + vec4[1] * mat[1][colindex] + vec4[2] * mat[2][colindex] + vec4[3] * mat[3][colindex]) /*!Transpose matrix times vector v is a vec3f and m is a mat4f<br> */ -#define INV_MAT_DOT_VEC_3X3(p,m,v) \ -{ \ - p[0] = MAT_DOT_COL(m,v,0); \ - p[1] = MAT_DOT_COL(m,v,1); \ - p[2] = MAT_DOT_COL(m,v,2); \ -}\ - - - -#endif // GIM_VECTOR_H_INCLUDED +#define INV_MAT_DOT_VEC_3X3(p, m, v) \ + { \ + p[0] = MAT_DOT_COL(m, v, 0); \ + p[1] = MAT_DOT_COL(m, v, 1); \ + p[2] = MAT_DOT_COL(m, v, 2); \ + } + +#endif // GIM_VECTOR_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_math.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_math.h index 939079e104..3c4f821a72 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_math.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_math.h @@ -34,8 +34,6 @@ email: projectileman@yahoo.com #include "LinearMath/btScalar.h" - - #define GREAL btScalar #define GREAL2 double #define GINT int @@ -45,8 +43,6 @@ email: projectileman@yahoo.com #define GINT64 long long #define GUINT64 unsigned long long - - #define G_PI 3.14159265358979f #define G_HALF_PI 1.5707963f //267948966 @@ -54,16 +50,14 @@ email: projectileman@yahoo.com //71795864 #define G_ROOT3 1.73205f #define G_ROOT2 1.41421f -#define G_UINT_INFINITY 0xffffffff //!< A very very high value +#define G_UINT_INFINITY 0xffffffff //!< A very very high value #define G_REAL_INFINITY FLT_MAX -#define G_SIGN_BITMASK 0x80000000 +#define G_SIGN_BITMASK 0x80000000 #define G_EPSILON SIMD_EPSILON - - enum GIM_SCALAR_TYPES { - G_STYPE_REAL =0, + G_STYPE_REAL = 0, G_STYPE_REAL2, G_STYPE_SHORT, G_STYPE_USHORT, @@ -73,85 +67,82 @@ enum GIM_SCALAR_TYPES G_STYPE_UINT64 }; - - -#define G_DEGTORAD(X) ((X)*3.1415926f/180.0f) -#define G_RADTODEG(X) ((X)*180.0f/3.1415926f) +#define G_DEGTORAD(X) ((X)*3.1415926f / 180.0f) +#define G_RADTODEG(X) ((X)*180.0f / 3.1415926f) //! Integer representation of a floating-point value. -#define GIM_IR(x) ((GUINT&)(x)) +#define GIM_IR(x) ((GUINT&)(x)) //! Signed integer representation of a floating-point value. -#define GIM_SIR(x) ((GINT&)(x)) +#define GIM_SIR(x) ((GINT&)(x)) //! Absolute integer representation of a floating-point value -#define GIM_AIR(x) (GIM_IR(x)&0x7fffffff) +#define GIM_AIR(x) (GIM_IR(x) & 0x7fffffff) //! Floating-point representation of an integer value. -#define GIM_FR(x) ((GREAL&)(x)) +#define GIM_FR(x) ((GREAL&)(x)) -#define GIM_MAX(a,b) (a<b?b:a) -#define GIM_MIN(a,b) (a>b?b:a) +#define GIM_MAX(a, b) (a < b ? b : a) +#define GIM_MIN(a, b) (a > b ? b : a) -#define GIM_MAX3(a,b,c) GIM_MAX(a,GIM_MAX(b,c)) -#define GIM_MIN3(a,b,c) GIM_MIN(a,GIM_MIN(b,c)) +#define GIM_MAX3(a, b, c) GIM_MAX(a, GIM_MAX(b, c)) +#define GIM_MIN3(a, b, c) GIM_MIN(a, GIM_MIN(b, c)) -#define GIM_IS_ZERO(value) (value < G_EPSILON && value > -G_EPSILON) +#define GIM_IS_ZERO(value) (value < G_EPSILON && value > -G_EPSILON) #define GIM_IS_NEGATIVE(value) (value <= -G_EPSILON) #define GIM_IS_POSISITVE(value) (value >= G_EPSILON) -#define GIM_NEAR_EQUAL(v1,v2) GIM_IS_ZERO((v1-v2)) +#define GIM_NEAR_EQUAL(v1, v2) GIM_IS_ZERO((v1 - v2)) ///returns a clamped number -#define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number)) +#define GIM_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number)) -#define GIM_GREATER(x, y) btFabs(x) > (y) +#define GIM_GREATER(x, y) btFabs(x) > (y) ///Swap numbers -#define GIM_SWAP_NUMBERS(a,b){ \ - a = a+b; \ - b = a-b; \ - a = a-b; \ -}\ - -#define GIM_INV_SQRT(va,isva)\ -{\ - if(va<=0.0000001f)\ - {\ - isva = G_REAL_INFINITY;\ - }\ - else\ - {\ - GREAL _x = va * 0.5f;\ - GUINT _y = 0x5f3759df - ( GIM_IR(va) >> 1);\ - isva = GIM_FR(_y);\ - isva = isva * ( 1.5f - ( _x * isva * isva ) );\ - }\ -}\ - -#define GIM_SQRT(va,sva)\ -{\ - GIM_INV_SQRT(va,sva);\ - sva = 1.0f/sva;\ -}\ +#define GIM_SWAP_NUMBERS(a, b) \ + { \ + a = a + b; \ + b = a - b; \ + a = a - b; \ + } + +#define GIM_INV_SQRT(va, isva) \ + { \ + if (va <= 0.0000001f) \ + { \ + isva = G_REAL_INFINITY; \ + } \ + else \ + { \ + GREAL _x = va * 0.5f; \ + GUINT _y = 0x5f3759df - (GIM_IR(va) >> 1); \ + isva = GIM_FR(_y); \ + isva = isva * (1.5f - (_x * isva * isva)); \ + } \ + } + +#define GIM_SQRT(va, sva) \ + { \ + GIM_INV_SQRT(va, sva); \ + sva = 1.0f / sva; \ + } //! Computes 1.0f / sqrtf(x). Comes from Quake3. See http://www.magic-software.com/3DGEDInvSqrt.html inline GREAL gim_inv_sqrt(GREAL f) { - GREAL r; - GIM_INV_SQRT(f,r); - return r; + GREAL r; + GIM_INV_SQRT(f, r); + return r; } inline GREAL gim_sqrt(GREAL f) { - GREAL r; - GIM_SQRT(f,r); - return r; + GREAL r; + GIM_SQRT(f, r); + return r; } - - -#endif // GIM_MATH_H_INCLUDED +#endif // GIM_MATH_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_memory.cpp b/thirdparty/bullet/BulletCollision/Gimpact/gim_memory.cpp index 1636eb7867..9e29ab91d6 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_memory.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_memory.cpp @@ -27,7 +27,6 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_memory.h" #include "stdlib.h" @@ -40,52 +39,49 @@ static gim_alloca_function *g_allocafn = 0; static gim_realloc_function *g_reallocfn = 0; static gim_free_function *g_freefn = 0; -void gim_set_alloc_handler (gim_alloc_function *fn) +void gim_set_alloc_handler(gim_alloc_function *fn) { - g_allocfn = fn; + g_allocfn = fn; } -void gim_set_alloca_handler (gim_alloca_function *fn) +void gim_set_alloca_handler(gim_alloca_function *fn) { - g_allocafn = fn; + g_allocafn = fn; } -void gim_set_realloc_handler (gim_realloc_function *fn) +void gim_set_realloc_handler(gim_realloc_function *fn) { - g_reallocfn = fn; + g_reallocfn = fn; } -void gim_set_free_handler (gim_free_function *fn) +void gim_set_free_handler(gim_free_function *fn) { - g_freefn = fn; + g_freefn = fn; } gim_alloc_function *gim_get_alloc_handler() { - return g_allocfn; + return g_allocfn; } gim_alloca_function *gim_get_alloca_handler() { - return g_allocafn; + return g_allocafn; } - -gim_realloc_function *gim_get_realloc_handler () +gim_realloc_function *gim_get_realloc_handler() { - return g_reallocfn; + return g_reallocfn; } - -gim_free_function *gim_get_free_handler () +gim_free_function *gim_get_free_handler() { - return g_freefn; + return g_freefn; } - -void * gim_alloc(size_t size) +void *gim_alloc(size_t size) { - void * ptr; + void *ptr; if (g_allocfn) { ptr = g_allocfn(size); @@ -93,27 +89,29 @@ void * gim_alloc(size_t size) else { #ifdef GIM_SIMD_MEMORY - ptr = btAlignedAlloc(size,16); + ptr = btAlignedAlloc(size, 16); #else ptr = malloc(size); #endif } - return ptr; + return ptr; } -void * gim_alloca(size_t size) +void *gim_alloca(size_t size) { - if (g_allocafn) return g_allocafn(size); else return gim_alloc(size); + if (g_allocafn) + return g_allocafn(size); + else + return gim_alloc(size); } - -void * gim_realloc(void *ptr, size_t oldsize, size_t newsize) +void *gim_realloc(void *ptr, size_t oldsize, size_t newsize) { - void * newptr = gim_alloc(newsize); - size_t copysize = oldsize<newsize?oldsize:newsize; - gim_simd_memcpy(newptr,ptr,copysize); - gim_free(ptr); - return newptr; + void *newptr = gim_alloc(newsize); + size_t copysize = oldsize < newsize ? oldsize : newsize; + gim_simd_memcpy(newptr, ptr, copysize); + gim_free(ptr); + return newptr; } void gim_free(void *ptr) @@ -121,15 +119,14 @@ void gim_free(void *ptr) if (!ptr) return; if (g_freefn) { - g_freefn(ptr); + g_freefn(ptr); } else { - #ifdef GIM_SIMD_MEMORY +#ifdef GIM_SIMD_MEMORY btAlignedFree(ptr); - #else +#else free(ptr); - #endif +#endif } } - diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_memory.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_memory.h index e203888a1e..fffbfa23d8 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_memory.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_memory.h @@ -32,93 +32,84 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ - #include "gim_math.h" #include <string.h> #ifdef PREFETCH -#include <xmmintrin.h> // for prefetch -#define pfval 64 -#define pfval2 128 +#include <xmmintrin.h> // for prefetch +#define pfval 64 +#define pfval2 128 //! Prefetch 64 -#define pf(_x,_i) _mm_prefetch((void *)(_x + _i + pfval), 0) +#define pf(_x, _i) _mm_prefetch((void *)(_x + _i + pfval), 0) //! Prefetch 128 -#define pf2(_x,_i) _mm_prefetch((void *)(_x + _i + pfval2), 0) +#define pf2(_x, _i) _mm_prefetch((void *)(_x + _i + pfval2), 0) #else //! Prefetch 64 -#define pf(_x,_i) +#define pf(_x, _i) //! Prefetch 128 -#define pf2(_x,_i) +#define pf2(_x, _i) #endif - ///Functions for manip packed arrays of numbers -#define GIM_COPY_ARRAYS(dest_array,source_array,element_count)\ -{\ - for (GUINT _i_=0;_i_<element_count ;++_i_)\ - {\ - dest_array[_i_] = source_array[_i_];\ - }\ -}\ - -#define GIM_COPY_ARRAYS_1(dest_array,source_array,element_count,copy_macro)\ -{\ - for (GUINT _i_=0;_i_<element_count ;++_i_)\ - {\ - copy_macro(dest_array[_i_],source_array[_i_]);\ - }\ -}\ - - -#define GIM_ZERO_ARRAY(array,element_count)\ -{\ - for (GUINT _i_=0;_i_<element_count ;++_i_)\ - {\ - array[_i_] = 0;\ - }\ -}\ - -#define GIM_CONSTANT_ARRAY(array,element_count,constant)\ -{\ - for (GUINT _i_=0;_i_<element_count ;++_i_)\ - {\ - array[_i_] = constant;\ - }\ -}\ - +#define GIM_COPY_ARRAYS(dest_array, source_array, element_count) \ + { \ + for (GUINT _i_ = 0; _i_ < element_count; ++_i_) \ + { \ + dest_array[_i_] = source_array[_i_]; \ + } \ + } + +#define GIM_COPY_ARRAYS_1(dest_array, source_array, element_count, copy_macro) \ + { \ + for (GUINT _i_ = 0; _i_ < element_count; ++_i_) \ + { \ + copy_macro(dest_array[_i_], source_array[_i_]); \ + } \ + } + +#define GIM_ZERO_ARRAY(array, element_count) \ + { \ + for (GUINT _i_ = 0; _i_ < element_count; ++_i_) \ + { \ + array[_i_] = 0; \ + } \ + } + +#define GIM_CONSTANT_ARRAY(array, element_count, constant) \ + { \ + for (GUINT _i_ = 0; _i_ < element_count; ++_i_) \ + { \ + array[_i_] = constant; \ + } \ + } ///Function prototypes to allocate and free memory. -typedef void * gim_alloc_function (size_t size); -typedef void * gim_alloca_function (size_t size);//Allocs on the heap -typedef void * gim_realloc_function (void *ptr, size_t oldsize, size_t newsize); -typedef void gim_free_function (void *ptr); - +typedef void *gim_alloc_function(size_t size); +typedef void *gim_alloca_function(size_t size); //Allocs on the heap +typedef void *gim_realloc_function(void *ptr, size_t oldsize, size_t newsize); +typedef void gim_free_function(void *ptr); ///Memory Function Handlers ///set new memory management functions. if fn is 0, the default handlers are used. -void gim_set_alloc_handler (gim_alloc_function *fn); -void gim_set_alloca_handler (gim_alloca_function *fn); -void gim_set_realloc_handler (gim_realloc_function *fn); -void gim_set_free_handler (gim_free_function *fn); - +void gim_set_alloc_handler(gim_alloc_function *fn); +void gim_set_alloca_handler(gim_alloca_function *fn); +void gim_set_realloc_handler(gim_realloc_function *fn); +void gim_set_free_handler(gim_free_function *fn); ///get current memory management functions. -gim_alloc_function *gim_get_alloc_handler (void); +gim_alloc_function *gim_get_alloc_handler(void); gim_alloca_function *gim_get_alloca_handler(void); -gim_realloc_function *gim_get_realloc_handler (void); -gim_free_function *gim_get_free_handler (void); - +gim_realloc_function *gim_get_realloc_handler(void); +gim_free_function *gim_get_free_handler(void); ///Standar Memory functions -void * gim_alloc(size_t size); -void * gim_alloca(size_t size); -void * gim_realloc(void *ptr, size_t oldsize, size_t newsize); +void *gim_alloc(size_t size); +void *gim_alloca(size_t size); +void *gim_realloc(void *ptr, size_t oldsize, size_t newsize); void gim_free(void *ptr); - - -#if defined (_WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__) - #define GIM_SIMD_MEMORY 1 +#if defined(_WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__) +#define GIM_SIMD_MEMORY 1 #endif //! SIMD POINTER INTEGER @@ -126,11 +117,10 @@ void gim_free(void *ptr); //! SIMD INTEGER SIZE #define SIMD_T_SIZE sizeof(SIMD_T) - -inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize) +inline void gim_simd_memcpy(void *dst, const void *src, size_t copysize) { #ifdef GIM_SIMD_MEMORY -/* + /* //'long long int' is incompatible with visual studio 6... //copy words SIMD_T * ui_src_ptr = (SIMD_T *)src; @@ -143,48 +133,45 @@ inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize) if(copysize==0) return; */ - char * c_src_ptr = (char *)src; - char * c_dst_ptr = (char *)dst; - while(copysize>0) - { - *(c_dst_ptr++) = *(c_src_ptr++); - copysize--; - } - return; + char *c_src_ptr = (char *)src; + char *c_dst_ptr = (char *)dst; + while (copysize > 0) + { + *(c_dst_ptr++) = *(c_src_ptr++); + copysize--; + } + return; #else - memcpy(dst,src,copysize); + memcpy(dst, src, copysize); #endif } - - -template<class T> -inline void gim_swap_elements(T* _array,size_t _i,size_t _j) +template <class T> +inline void gim_swap_elements(T *_array, size_t _i, size_t _j) { T _e_tmp_ = _array[_i]; _array[_i] = _array[_j]; _array[_j] = _e_tmp_; } - -template<class T> -inline void gim_swap_elements_memcpy(T* _array,size_t _i,size_t _j) +template <class T> +inline void gim_swap_elements_memcpy(T *_array, size_t _i, size_t _j) { char _e_tmp_[sizeof(T)]; - gim_simd_memcpy(_e_tmp_,&_array[_i],sizeof(T)); - gim_simd_memcpy(&_array[_i],&_array[_j],sizeof(T)); - gim_simd_memcpy(&_array[_j],_e_tmp_,sizeof(T)); + gim_simd_memcpy(_e_tmp_, &_array[_i], sizeof(T)); + gim_simd_memcpy(&_array[_i], &_array[_j], sizeof(T)); + gim_simd_memcpy(&_array[_j], _e_tmp_, sizeof(T)); } template <int SIZE> -inline void gim_swap_elements_ptr(char * _array,size_t _i,size_t _j) +inline void gim_swap_elements_ptr(char *_array, size_t _i, size_t _j) { char _e_tmp_[SIZE]; - _i*=SIZE; - _j*=SIZE; - gim_simd_memcpy(_e_tmp_,_array+_i,SIZE); - gim_simd_memcpy(_array+_i,_array+_j,SIZE); - gim_simd_memcpy(_array+_j,_e_tmp_,SIZE); + _i *= SIZE; + _j *= SIZE; + gim_simd_memcpy(_e_tmp_, _array + _i, SIZE); + gim_simd_memcpy(_array + _i, _array + _j, SIZE); + gim_simd_memcpy(_array + _j, _e_tmp_, SIZE); } -#endif // GIM_MEMORY_H_INCLUDED +#endif // GIM_MEMORY_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_radixsort.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_radixsort.h index c246ef1254..ff7907adca 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_radixsort.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_radixsort.h @@ -40,24 +40,22 @@ email: projectileman@yahoo.com //! Prototype for comparators class less_comparator { - public: - - template<class T,class Z> - inline int operator() ( const T& a, const Z& b ) +public: + template <class T, class Z> + inline int operator()(const T& a, const Z& b) { - return ( a<b?-1:(a>b?1:0)); + return (a < b ? -1 : (a > b ? 1 : 0)); } }; //! Prototype for comparators class integer_comparator { - public: - - template<class T> - inline int operator() ( const T& a, const T& b ) +public: + template <class T> + inline int operator()(const T& a, const T& b) { - return (int)(a-b); + return (int)(a - b); } }; @@ -65,20 +63,19 @@ class integer_comparator class uint_key_func { public: - template<class T> - inline GUINT operator()( const T& a) + template <class T> + inline GUINT operator()(const T& a) { return (GUINT)a; } }; - //!Prototype for copying elements class copy_elements_func { public: - template<class T> - inline void operator()(T& a,T& b) + template <class T> + inline void operator()(T& a, T& b) { a = b; } @@ -88,34 +85,33 @@ public: class memcopy_elements_func { public: - template<class T> - inline void operator()(T& a,T& b) + template <class T> + inline void operator()(T& a, T& b) { - gim_simd_memcpy(&a,&b,sizeof(T)); + gim_simd_memcpy(&a, &b, sizeof(T)); } }; - //! @{ struct GIM_RSORT_TOKEN { - GUINT m_key; - GUINT m_value; - GIM_RSORT_TOKEN() - { - } - GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken) - { - m_key = rtoken.m_key; - m_value = rtoken.m_value; - } + GUINT m_key; + GUINT m_value; + GIM_RSORT_TOKEN() + { + } + GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken) + { + m_key = rtoken.m_key; + m_value = rtoken.m_value; + } - inline bool operator <(const GIM_RSORT_TOKEN& other) const + inline bool operator<(const GIM_RSORT_TOKEN& other) const { return (m_key < other.m_key); } - inline bool operator >(const GIM_RSORT_TOKEN& other) const + inline bool operator>(const GIM_RSORT_TOKEN& other) const { return (m_key > other.m_key); } @@ -124,33 +120,28 @@ struct GIM_RSORT_TOKEN //! Prototype for comparators class GIM_RSORT_TOKEN_COMPARATOR { - public: - - inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b ) +public: + inline int operator()(const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b) { return (int)((a.m_key) - (b.m_key)); } }; - - #define kHist 2048 // ---- utils for accessing 11-bit quantities -#define D11_0(x) (x & 0x7FF) -#define D11_1(x) (x >> 11 & 0x7FF) -#define D11_2(x) (x >> 22 ) - - +#define D11_0(x) (x & 0x7FF) +#define D11_1(x) (x >> 11 & 0x7FF) +#define D11_2(x) (x >> 22) ///Radix sort for unsigned integer keys inline void gim_radix_sort_rtokens( - GIM_RSORT_TOKEN * array, - GIM_RSORT_TOKEN * sorted, GUINT element_count) + GIM_RSORT_TOKEN* array, + GIM_RSORT_TOKEN* sorted, GUINT element_count) { GUINT i; GUINT b0[kHist * 3]; - GUINT *b1 = b0 + kHist; - GUINT *b2 = b1 + kHist; + GUINT* b1 = b0 + kHist; + GUINT* b2 = b1 + kHist; for (i = 0; i < kHist * 3; ++i) { b0[i] = 0; @@ -159,10 +150,10 @@ inline void gim_radix_sort_rtokens( GUINT pos; for (i = 0; i < element_count; ++i) { - fi = array[i].m_key; - b0[D11_0(fi)] ++; - b1[D11_1(fi)] ++; - b2[D11_2(fi)] ++; + fi = array[i].m_key; + b0[D11_0(fi)]++; + b1[D11_1(fi)]++; + b2[D11_2(fi)]++; } { GUINT sum0 = 0, sum1 = 0, sum2 = 0; @@ -182,7 +173,7 @@ inline void gim_radix_sort_rtokens( } for (i = 0; i < element_count; ++i) { - fi = array[i].m_key; + fi = array[i].m_key; pos = D11_0(fi); pos = ++b0[pos]; sorted[pos].m_key = array[i].m_key; @@ -190,7 +181,7 @@ inline void gim_radix_sort_rtokens( } for (i = 0; i < element_count; ++i) { - fi = sorted[i].m_key; + fi = sorted[i].m_key; pos = D11_1(fi); pos = ++b1[pos]; array[pos].m_key = sorted[i].m_key; @@ -198,7 +189,7 @@ inline void gim_radix_sort_rtokens( } for (i = 0; i < element_count; ++i) { - fi = array[i].m_key; + fi = array[i].m_key; pos = D11_2(fi); pos = ++b2[pos]; sorted[pos].m_key = array[i].m_key; @@ -206,9 +197,6 @@ inline void gim_radix_sort_rtokens( } } - - - /// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN /*! *\param array Array of elements to sort @@ -216,21 +204,21 @@ inline void gim_radix_sort_rtokens( *\param element_count element count *\param uintkey_macro Functor which retrieves the integer representation of an array element */ -template<typename T, class GETKEY_CLASS> +template <typename T, class GETKEY_CLASS> void gim_radix_sort_array_tokens( - T* array , - GIM_RSORT_TOKEN * sorted_tokens, - GUINT element_count,GETKEY_CLASS uintkey_macro) + T* array, + GIM_RSORT_TOKEN* sorted_tokens, + GUINT element_count, GETKEY_CLASS uintkey_macro) { - GIM_RSORT_TOKEN * _unsorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count); - for (GUINT _i=0;_i<element_count;++_i) - { - _unsorted[_i].m_key = uintkey_macro(array[_i]); - _unsorted[_i].m_value = _i; - } - gim_radix_sort_rtokens(_unsorted,sorted_tokens,element_count); - gim_free(_unsorted); - gim_free(_unsorted); + GIM_RSORT_TOKEN* _unsorted = (GIM_RSORT_TOKEN*)gim_alloc(sizeof(GIM_RSORT_TOKEN) * element_count); + for (GUINT _i = 0; _i < element_count; ++_i) + { + _unsorted[_i].m_key = uintkey_macro(array[_i]); + _unsorted[_i].m_value = _i; + } + gim_radix_sort_rtokens(_unsorted, sorted_tokens, element_count); + gim_free(_unsorted); + gim_free(_unsorted); } /// Sorts array in place. For generic use @@ -241,21 +229,21 @@ void gim_radix_sort_array_tokens( \param get_uintkey_macro Macro for extract the Integer value of the element. Similar to SIMPLE_GET_UINTKEY \param copy_elements_macro Macro for copy elements, similar to SIMPLE_COPY_ELEMENTS */ -template<typename T, class GETKEY_CLASS, class COPY_CLASS> +template <typename T, class GETKEY_CLASS, class COPY_CLASS> void gim_radix_sort( - T * array, GUINT element_count, + T* array, GUINT element_count, GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro) { - GIM_RSORT_TOKEN * _sorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count); - gim_radix_sort_array_tokens(array,_sorted,element_count,get_uintkey_macro); - T * _original_array = (T *) gim_alloc(sizeof(T)*element_count); - gim_simd_memcpy(_original_array,array,sizeof(T)*element_count); - for (GUINT _i=0;_i<element_count;++_i) - { - copy_elements_macro(array[_i],_original_array[_sorted[_i].m_value]); - } - gim_free(_original_array); - gim_free(_sorted); + GIM_RSORT_TOKEN* _sorted = (GIM_RSORT_TOKEN*)gim_alloc(sizeof(GIM_RSORT_TOKEN) * element_count); + gim_radix_sort_array_tokens(array, _sorted, element_count, get_uintkey_macro); + T* _original_array = (T*)gim_alloc(sizeof(T) * element_count); + gim_simd_memcpy(_original_array, array, sizeof(T) * element_count); + for (GUINT _i = 0; _i < element_count; ++_i) + { + copy_elements_macro(array[_i], _original_array[_sorted[_i].m_value]); + } + gim_free(_original_array); + gim_free(_sorted); } //! Failsafe Iterative binary search, @@ -269,20 +257,20 @@ If the element is not found, it returns the nearest upper element position, may \param _found If true the value has found. Boolean \param _result_index the index of the found element, or if not found then it will get the index of the closest bigger value */ -template<class T, typename KEYCLASS, typename COMP_CLASS> -bool gim_binary_search_ex( - const T* _array, GUINT _start_i, - GUINT _end_i,GUINT & _result_index, - const KEYCLASS & _search_key, - COMP_CLASS _comp_macro) +template <class T, typename KEYCLASS, typename COMP_CLASS> +bool gim_binary_search_ex( + const T* _array, GUINT _start_i, + GUINT _end_i, GUINT& _result_index, + const KEYCLASS& _search_key, + COMP_CLASS _comp_macro) { GUINT _k; int _comp_result; GUINT _i = _start_i; - GUINT _j = _end_i+1; + GUINT _j = _end_i + 1; while (_i < _j) { - _k = (_j+_i-1)/2; + _k = (_j + _i - 1) / 2; _comp_result = _comp_macro(_array[_k], _search_key); if (_comp_result == 0) { @@ -291,7 +279,7 @@ bool gim_binary_search_ex( } else if (_comp_result < 0) { - _i = _k+1; + _i = _k + 1; } else { @@ -302,8 +290,6 @@ bool gim_binary_search_ex( return false; } - - //! Failsafe Iterative binary search,Template version /*! If the element is not found, it returns the nearest upper element position, may be the further position after the last element. @@ -314,26 +300,26 @@ If the element is not found, it returns the nearest upper element position, may \param _result_index the index of the found element, or if not found then it will get the index of the closest bigger value \return true if found, else false */ -template<class T> +template <class T> bool gim_binary_search( - const T*_array,GUINT _start_i, - GUINT _end_i,const T & _search_key, - GUINT & _result_index) + const T* _array, GUINT _start_i, + GUINT _end_i, const T& _search_key, + GUINT& _result_index) { GUINT _i = _start_i; - GUINT _j = _end_i+1; + GUINT _j = _end_i + 1; GUINT _k; - while(_i < _j) + while (_i < _j) { - _k = (_j+_i-1)/2; - if(_array[_k]==_search_key) + _k = (_j + _i - 1) / 2; + if (_array[_k] == _search_key) { _result_index = _k; return true; } - else if (_array[_k]<_search_key) + else if (_array[_k] < _search_key) { - _i = _k+1; + _i = _k + 1; } else { @@ -344,27 +330,25 @@ bool gim_binary_search( return false; } - - ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ template <typename T, typename COMP_CLASS> -void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc) +void gim_down_heap(T* pArr, GUINT k, GUINT n, COMP_CLASS CompareFunc) { /* PRE: a[k+1..N] is a heap */ /* POST: a[k..N] is a heap */ T temp = pArr[k - 1]; /* k has child(s) */ - while (k <= n/2) + while (k <= n / 2) { - int child = 2*k; + int child = 2 * k; - if ((child < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0) + if ((child < (int)n) && CompareFunc(pArr[child - 1], pArr[child]) < 0) { child++; } /* pick larger child */ - if (CompareFunc(temp , pArr[child - 1])<0) + if (CompareFunc(temp, pArr[child - 1]) < 0) { /* move child up */ pArr[k - 1] = pArr[child - 1]; @@ -378,29 +362,25 @@ void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc) pArr[k - 1] = temp; } /*downHeap*/ - template <typename T, typename COMP_CLASS> -void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc) +void gim_heap_sort(T* pArr, GUINT element_count, COMP_CLASS CompareFunc) { /* sort a[0..N-1], N.B. 0 to N-1 */ GUINT k; GUINT n = element_count; - for (k = n/2; k > 0; k--) + for (k = n / 2; k > 0; k--) { gim_down_heap(pArr, k, n, CompareFunc); } /* a[1..N] is now a heap */ - while ( n>=2 ) + while (n >= 2) { - gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */ + gim_swap_elements(pArr, 0, n - 1); /* largest of a[0..n-1] */ --n; /* restore a[1..i-1] heap */ gim_down_heap(pArr, 1, n, CompareFunc); } } - - - -#endif // GIM_RADIXSORT_H_INCLUDED +#endif // GIM_RADIXSORT_H_INCLUDED diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_tri_collision.cpp b/thirdparty/bullet/BulletCollision/Gimpact/gim_tri_collision.cpp index f9727e1d53..8d83e95da4 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_tri_collision.cpp +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_tri_collision.cpp @@ -33,15 +33,13 @@ email: projectileman@yahoo.com #include "gim_tri_collision.h" - #define TRI_LOCAL_EPSILON 0.000001f #define MIN_EDGE_EDGE_DIS 0.00001f - class GIM_TRIANGLE_CALCULATION_CACHE { public: - GREAL margin; + GREAL margin; btVector3 tu_vertices[3]; btVector3 tv_vertices[3]; btVector4 tu_plane; @@ -55,46 +53,47 @@ public: GREAL du0du2; GREAL dv[4]; GREAL dv0dv1; - GREAL dv0dv2; + GREAL dv0dv2; btVector3 temp_points[MAX_TRI_CLIPPING]; btVector3 temp_points1[MAX_TRI_CLIPPING]; btVector3 contact_points[MAX_TRI_CLIPPING]; - - //! if returns false, the faces are paralele SIMD_FORCE_INLINE bool compute_intervals( - const GREAL &D0, - const GREAL &D1, - const GREAL &D2, - const GREAL &D0D1, - const GREAL &D0D2, - GREAL & scale_edge0, - GREAL & scale_edge1, - GUINT &edge_index0, - GUINT &edge_index1) + const GREAL &D0, + const GREAL &D1, + const GREAL &D2, + const GREAL &D0D1, + const GREAL &D0D2, + GREAL &scale_edge0, + GREAL &scale_edge1, + GUINT &edge_index0, + GUINT &edge_index1) { - if(D0D1>0.0f) + if (D0D1 > 0.0f) { /* here we know that D0D2<=0.0 */ /* that is D0, D1 are on the same side, D2 on the other or on the plane */ - scale_edge0 = -D2/(D0-D2); - scale_edge1 = -D1/(D2-D1); - edge_index0 = 2;edge_index1 = 1; + scale_edge0 = -D2 / (D0 - D2); + scale_edge1 = -D1 / (D2 - D1); + edge_index0 = 2; + edge_index1 = 1; } - else if(D0D2>0.0f) + else if (D0D2 > 0.0f) { /* here we know that d0d1<=0.0 */ - scale_edge0 = -D0/(D1-D0); - scale_edge1 = -D1/(D2-D1); - edge_index0 = 0;edge_index1 = 1; + scale_edge0 = -D0 / (D1 - D0); + scale_edge1 = -D1 / (D2 - D1); + edge_index0 = 0; + edge_index1 = 1; } - else if(D1*D2>0.0f || D0!=0.0f) + else if (D1 * D2 > 0.0f || D0 != 0.0f) { /* here we know that d0d1<=0.0 or that D0!=0.0 */ - scale_edge0 = -D0/(D1-D0); - scale_edge1 = -D2/(D0-D2); - edge_index0 = 0 ;edge_index1 = 2; + scale_edge0 = -D0 / (D1 - D0); + scale_edge1 = -D2 / (D0 - D2); + edge_index0 = 0; + edge_index1 = 2; } else { @@ -103,46 +102,44 @@ public: return true; } - //! clip triangle /*! */ SIMD_FORCE_INLINE GUINT clip_triangle( - const btVector4 & tri_plane, - const btVector3 * tripoints, - const btVector3 * srcpoints, - btVector3 * clip_points) + const btVector4 &tri_plane, + const btVector3 *tripoints, + const btVector3 *srcpoints, + btVector3 *clip_points) { // edge 0 btVector4 edgeplane; - EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane); + EDGE_PLANE(tripoints[0], tripoints[1], tri_plane, edgeplane); GUINT clipped_count = PLANE_CLIP_TRIANGLE3D( - edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points); + edgeplane, srcpoints[0], srcpoints[1], srcpoints[2], temp_points); - if(clipped_count == 0) return 0; + if (clipped_count == 0) return 0; // edge 1 - EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane); + EDGE_PLANE(tripoints[1], tripoints[2], tri_plane, edgeplane); clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points,clipped_count,temp_points1); + edgeplane, temp_points, clipped_count, temp_points1); - if(clipped_count == 0) return 0; + if (clipped_count == 0) return 0; // edge 2 - EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane); + EDGE_PLANE(tripoints[2], tripoints[0], tri_plane, edgeplane); clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points1,clipped_count,clip_points); + edgeplane, temp_points1, clipped_count, clip_points); return clipped_count; - /*GUINT i0 = (tri_plane.closestAxis()+1)%3; GUINT i1 = (i0+1)%3; // edge 0 @@ -172,13 +169,13 @@ public: } SIMD_FORCE_INLINE void sort_isect( - GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1) + GREAL &isect0, GREAL &isect1, GUINT &e0, GUINT &e1, btVector3 &vec0, btVector3 &vec1) { - if(isect1<isect0) + if (isect1 < isect0) { //swap - GIM_SWAP_NUMBERS(isect0,isect1); - GIM_SWAP_NUMBERS(e0,e1); + GIM_SWAP_NUMBERS(isect0, isect1); + GIM_SWAP_NUMBERS(e0, e1); btVector3 tmp = vec0; vec0 = vec1; vec1 = tmp; @@ -202,53 +199,52 @@ public: // Compute direction of intersection line edge_edge_dir = tu_plane.cross(tv_plane); GREAL Dlen; - VEC_LENGTH(edge_edge_dir,Dlen); + VEC_LENGTH(edge_edge_dir, Dlen); - if(Dlen<0.0001) + if (Dlen < 0.0001) { - return 0; //faces near paralele + return 0; //faces near paralele } - edge_edge_dir*= 1/Dlen;//normalize - + edge_edge_dir *= 1 / Dlen; //normalize // Compute interval for triangle 1 - GUINT tu_e0,tu_e1;//edge indices - GREAL tu_scale_e0,tu_scale_e1;//edge scale - if(!compute_intervals(du[0],du[1],du[2], - du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0; + GUINT tu_e0, tu_e1; //edge indices + GREAL tu_scale_e0, tu_scale_e1; //edge scale + if (!compute_intervals(du[0], du[1], du[2], + du0du1, du0du2, tu_scale_e0, tu_scale_e1, tu_e0, tu_e1)) return 0; // Compute interval for triangle 2 - GUINT tv_e0,tv_e1;//edge indices - GREAL tv_scale_e0,tv_scale_e1;//edge scale + GUINT tv_e0, tv_e1; //edge indices + GREAL tv_scale_e0, tv_scale_e1; //edge scale - if(!compute_intervals(dv[0],dv[1],dv[2], - dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0; + if (!compute_intervals(dv[0], dv[1], dv[2], + dv0dv1, dv0dv2, tv_scale_e0, tv_scale_e1, tv_e0, tv_e1)) return 0; //proyected vertices - btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0); - btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1); + btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0 + 1) % 3], tu_scale_e0); + btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1 + 1) % 3], tu_scale_e1); - btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0); - btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1); + btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0 + 1) % 3], tv_scale_e0); + btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1 + 1) % 3], tv_scale_e1); //proyected intervals - GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)}; - GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)}; + GREAL isect_u[] = {up_e0.dot(edge_edge_dir), up_e1.dot(edge_edge_dir)}; + GREAL isect_v[] = {vp_e0.dot(edge_edge_dir), vp_e1.dot(edge_edge_dir)}; - sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1); - sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1); + sort_isect(isect_u[0], isect_u[1], tu_e0, tu_e1, up_e0, up_e1); + sort_isect(isect_v[0], isect_v[1], tv_e0, tv_e1, vp_e0, vp_e1); - const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint - const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint + const GREAL midpoint_u = 0.5f * (isect_u[0] + isect_u[1]); // midpoint + const GREAL midpoint_v = 0.5f * (isect_v[0] + isect_v[1]); // midpoint - if(midpoint_u<midpoint_v) + if (midpoint_u < midpoint_v) { - if(isect_u[1]>=isect_v[1]) // face U casts face V + if (isect_u[1] >= isect_v[1]) // face U casts face V { return 1; } - else if(isect_v[0]<=isect_u[0]) // face V casts face U + else if (isect_v[0] <= isect_u[0]) // face V casts face U { return 2; } @@ -257,32 +253,31 @@ public: closest_point_v = vp_e0; // calc edges and separation - if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead + if (isect_u[1] + MIN_EDGE_EDGE_DIS < isect_v[0]) //calc distance between two lines instead { SEGMENT_COLLISION( - tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3], - tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3], + tu_vertices[tu_e1], tu_vertices[(tu_e1 + 1) % 3], + tv_vertices[tv_e0], tv_vertices[(tv_e0 + 1) % 3], closest_point_u, closest_point_v); - edge_edge_dir = closest_point_u-closest_point_v; - VEC_LENGTH(edge_edge_dir,distances[2]); - edge_edge_dir *= 1.0f/distances[2];// normalize + edge_edge_dir = closest_point_u - closest_point_v; + VEC_LENGTH(edge_edge_dir, distances[2]); + edge_edge_dir *= 1.0f / distances[2]; // normalize } else { - distances[2] = isect_v[0]-isect_u[1];//distance negative - //edge_edge_dir *= -1.0f; //normal pointing from V to U + distances[2] = isect_v[0] - isect_u[1]; //distance negative + //edge_edge_dir *= -1.0f; //normal pointing from V to U } - } else { - if(isect_v[1]>=isect_u[1]) // face V casts face U + if (isect_v[1] >= isect_u[1]) // face V casts face U { return 2; } - else if(isect_u[0]<=isect_v[0]) // face U casts face V + else if (isect_u[0] <= isect_v[0]) // face U casts face V { return 1; } @@ -291,41 +286,39 @@ public: closest_point_v = vp_e1; // calc edges and separation - if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead + if (isect_v[1] + MIN_EDGE_EDGE_DIS < isect_u[0]) //calc distance between two lines instead { SEGMENT_COLLISION( - tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3], - tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3], + tu_vertices[tu_e0], tu_vertices[(tu_e0 + 1) % 3], + tv_vertices[tv_e1], tv_vertices[(tv_e1 + 1) % 3], closest_point_u, closest_point_v); - edge_edge_dir = closest_point_u-closest_point_v; - VEC_LENGTH(edge_edge_dir,distances[2]); - edge_edge_dir *= 1.0f/distances[2];// normalize + edge_edge_dir = closest_point_u - closest_point_v; + VEC_LENGTH(edge_edge_dir, distances[2]); + edge_edge_dir *= 1.0f / distances[2]; // normalize } else { - distances[2] = isect_u[0]-isect_v[1];//distance negative - //edge_edge_dir *= -1.0f; //normal pointing from V to U + distances[2] = isect_u[0] - isect_v[1]; //distance negative + //edge_edge_dir *= -1.0f; //normal pointing from V to U } } return 3; } - //! collides by two sides SIMD_FORCE_INLINE bool triangle_collision( - const btVector3 & u0, - const btVector3 & u1, - const btVector3 & u2, - GREAL margin_u, - const btVector3 & v0, - const btVector3 & v1, - const btVector3 & v2, - GREAL margin_v, - GIM_TRIANGLE_CONTACT_DATA & contacts) + const btVector3 &u0, + const btVector3 &u1, + const btVector3 &u2, + GREAL margin_u, + const btVector3 &v0, + const btVector3 &v1, + const btVector3 &v2, + GREAL margin_v, + GIM_TRIANGLE_CONTACT_DATA &contacts) { - margin = margin_u + margin_v; tu_vertices[0] = u0; @@ -339,103 +332,99 @@ public: //create planes // plane v vs U points - TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane); - - du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]); - du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]); - du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]); + TRIANGLE_PLANE(tv_vertices[0], tv_vertices[1], tv_vertices[2], tv_plane); + du[0] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[0]); + du[1] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[1]); + du[2] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[2]); du0du1 = du[0] * du[1]; du0du2 = du[0] * du[2]; - - if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ? + if (du0du1 > 0.0f && du0du2 > 0.0f) // same sign on all of them + not equal 0 ? { - if(du[0]<0) //we need test behind the triangle plane + if (du[0] < 0) //we need test behind the triangle plane { - distances[0] = GIM_MAX3(du[0],du[1],du[2]); + distances[0] = GIM_MAX3(du[0], du[1], du[2]); distances[0] = -distances[0]; - if(distances[0]>margin) return false; //never intersect + if (distances[0] > margin) return false; //never intersect //reorder triangle v - VEC_SWAP(tv_vertices[0],tv_vertices[1]); - VEC_SCALE_4(tv_plane,-1.0f,tv_plane); + VEC_SWAP(tv_vertices[0], tv_vertices[1]); + VEC_SCALE_4(tv_plane, -1.0f, tv_plane); } else { - distances[0] = GIM_MIN3(du[0],du[1],du[2]); - if(distances[0]>margin) return false; //never intersect + distances[0] = GIM_MIN3(du[0], du[1], du[2]); + if (distances[0] > margin) return false; //never intersect } } else { //Look if we need to invert the triangle - distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid + distances[0] = (du[0] + du[1] + du[2]) / 3.0f; //centroid - if(distances[0]<0.0f) + if (distances[0] < 0.0f) { //reorder triangle v - VEC_SWAP(tv_vertices[0],tv_vertices[1]); - VEC_SCALE_4(tv_plane,-1.0f,tv_plane); + VEC_SWAP(tv_vertices[0], tv_vertices[1]); + VEC_SCALE_4(tv_plane, -1.0f, tv_plane); - distances[0] = GIM_MAX3(du[0],du[1],du[2]); + distances[0] = GIM_MAX3(du[0], du[1], du[2]); distances[0] = -distances[0]; } else { - distances[0] = GIM_MIN3(du[0],du[1],du[2]); + distances[0] = GIM_MIN3(du[0], du[1], du[2]); } } - // plane U vs V points - TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane); + TRIANGLE_PLANE(tu_vertices[0], tu_vertices[1], tu_vertices[2], tu_plane); - dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]); - dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]); - dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]); + dv[0] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[0]); + dv[1] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[1]); + dv[2] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[2]); dv0dv1 = dv[0] * dv[1]; dv0dv2 = dv[0] * dv[2]; - - if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ? + if (dv0dv1 > 0.0f && dv0dv2 > 0.0f) // same sign on all of them + not equal 0 ? { - if(dv[0]<0) //we need test behind the triangle plane + if (dv[0] < 0) //we need test behind the triangle plane { - distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]); distances[1] = -distances[1]; - if(distances[1]>margin) return false; //never intersect + if (distances[1] > margin) return false; //never intersect //reorder triangle u - VEC_SWAP(tu_vertices[0],tu_vertices[1]); - VEC_SCALE_4(tu_plane,-1.0f,tu_plane); + VEC_SWAP(tu_vertices[0], tu_vertices[1]); + VEC_SCALE_4(tu_plane, -1.0f, tu_plane); } else { - distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); - if(distances[1]>margin) return false; //never intersect + distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]); + if (distances[1] > margin) return false; //never intersect } } else { //Look if we need to invert the triangle - distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid + distances[1] = (dv[0] + dv[1] + dv[2]) / 3.0f; //centroid - if(distances[1]<0.0f) + if (distances[1] < 0.0f) { //reorder triangle v - VEC_SWAP(tu_vertices[0],tu_vertices[1]); - VEC_SCALE_4(tu_plane,-1.0f,tu_plane); + VEC_SWAP(tu_vertices[0], tu_vertices[1]); + VEC_SCALE_4(tu_plane, -1.0f, tu_plane); - distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]); distances[1] = -distances[1]; } else { - distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]); } } @@ -448,47 +437,44 @@ public: } else {*/ - bl = 0; - if(distances[0]<distances[1]) bl = 1; + bl = 0; + if (distances[0] < distances[1]) bl = 1; //} - if(bl==2) //edge edge separation + if (bl == 2) //edge edge separation { - if(distances[2]>margin) return false; + if (distances[2] > margin) return false; contacts.m_penetration_depth = -distances[2] + margin; contacts.m_points[0] = closest_point_v; contacts.m_point_count = 1; - VEC_COPY(contacts.m_separating_normal,edge_edge_dir); + VEC_COPY(contacts.m_separating_normal, edge_edge_dir); return true; } //clip face against other - GUINT point_count; //TODO - if(bl == 0) //clip U points against V + if (bl == 0) //clip U points against V { - point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points); - if(point_count == 0) return false; - contacts.merge_points(tv_plane,margin,contact_points,point_count); + point_count = clip_triangle(tv_plane, tv_vertices, tu_vertices, contact_points); + if (point_count == 0) return false; + contacts.merge_points(tv_plane, margin, contact_points, point_count); } - else //clip V points against U + else //clip V points against U { - point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points); - if(point_count == 0) return false; - contacts.merge_points(tu_plane,margin,contact_points,point_count); + point_count = clip_triangle(tu_plane, tu_vertices, tv_vertices, contact_points); + if (point_count == 0) return false; + contacts.merge_points(tu_plane, margin, contact_points, point_count); contacts.m_separating_normal *= -1.f; } - if(contacts.m_point_count == 0) return false; + if (contacts.m_point_count == 0) return false; return true; } - }; - /*class GIM_TRIANGLE_CALCULATION_CACHE { public: @@ -621,20 +607,13 @@ public: };*/ - - bool GIM_TRIANGLE::collide_triangle_hard_test( - const GIM_TRIANGLE & other, - GIM_TRIANGLE_CONTACT_DATA & contact_data) const + const GIM_TRIANGLE &other, + GIM_TRIANGLE_CONTACT_DATA &contact_data) const { - GIM_TRIANGLE_CALCULATION_CACHE calc_cache; + GIM_TRIANGLE_CALCULATION_CACHE calc_cache; return calc_cache.triangle_collision( - m_vertices[0],m_vertices[1],m_vertices[2],m_margin, - other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin, - contact_data); - + m_vertices[0], m_vertices[1], m_vertices[2], m_margin, + other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], other.m_margin, + contact_data); } - - - - diff --git a/thirdparty/bullet/BulletCollision/Gimpact/gim_tri_collision.h b/thirdparty/bullet/BulletCollision/Gimpact/gim_tri_collision.h index 267f806e7e..e6d4bf5470 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/gim_tri_collision.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/gim_tri_collision.h @@ -36,8 +36,6 @@ email: projectileman@yahoo.com #include "gim_box_collision.h" #include "gim_clip_polygon.h" - - #ifndef MAX_TRI_CLIPPING #define MAX_TRI_CLIPPING 16 #endif @@ -45,18 +43,18 @@ email: projectileman@yahoo.com //! Structure for collision struct GIM_TRIANGLE_CONTACT_DATA { - GREAL m_penetration_depth; - GUINT m_point_count; - btVector4 m_separating_normal; - btVector3 m_points[MAX_TRI_CLIPPING]; + GREAL m_penetration_depth; + GUINT m_point_count; + btVector4 m_separating_normal; + btVector3 m_points[MAX_TRI_CLIPPING]; - SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other) + SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA &other) { m_penetration_depth = other.m_penetration_depth; m_separating_normal = other.m_separating_normal; m_point_count = other.m_point_count; GUINT i = m_point_count; - while(i--) + while (i--) { m_points[i] = other.m_points[i]; } @@ -66,39 +64,36 @@ struct GIM_TRIANGLE_CONTACT_DATA { } - GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other) + GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA &other) { copy_from(other); } - - - - //! classify points that are closer - template<typename DISTANCE_FUNC,typename CLASS_PLANE> - SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane, - GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func) - { - m_point_count = 0; - m_penetration_depth= -1000.0f; + //! classify points that are closer + template <typename DISTANCE_FUNC, typename CLASS_PLANE> + SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE &plane, + GREAL margin, const btVector3 *points, GUINT point_count, DISTANCE_FUNC distance_func) + { + m_point_count = 0; + m_penetration_depth = -1000.0f; GUINT point_indices[MAX_TRI_CLIPPING]; GUINT _k; - for(_k=0;_k<point_count;_k++) + for (_k = 0; _k < point_count; _k++) { - GREAL _dist = -distance_func(plane,points[_k]) + margin; + GREAL _dist = -distance_func(plane, points[_k]) + margin; - if(_dist>=0.0f) + if (_dist >= 0.0f) { - if(_dist>m_penetration_depth) + if (_dist > m_penetration_depth) { m_penetration_depth = _dist; point_indices[0] = _k; - m_point_count=1; + m_point_count = 1; } - else if((_dist+G_EPSILON)>=m_penetration_depth) + else if ((_dist + G_EPSILON) >= m_penetration_depth) { point_indices[m_point_count] = _k; m_point_count++; @@ -106,88 +101,87 @@ struct GIM_TRIANGLE_CONTACT_DATA } } - for( _k=0;_k<m_point_count;_k++) + for (_k = 0; _k < m_point_count; _k++) { m_points[_k] = points[point_indices[_k]]; } } //! classify points that are closer - SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin, - const btVector3 * points, GUINT point_count) + SIMD_FORCE_INLINE void merge_points(const btVector4 &plane, GREAL margin, + const btVector3 *points, GUINT point_count) { m_separating_normal = plane; mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC()); } }; - //! Class for colliding triangles class GIM_TRIANGLE { public: btScalar m_margin; - btVector3 m_vertices[3]; - - GIM_TRIANGLE():m_margin(0.1f) - { - } - - SIMD_FORCE_INLINE GIM_AABB get_box() const - { - return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin); - } - - SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const - { - TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal); - } - - SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const - { - TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);; - } - - SIMD_FORCE_INLINE void apply_transform(const btTransform & trans) - { - m_vertices[0] = trans(m_vertices[0]); - m_vertices[1] = trans(m_vertices[1]); - m_vertices[2] = trans(m_vertices[2]); - } - - SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane) const - { - const btVector3 & e0 = m_vertices[edge_index]; - const btVector3 & e1 = m_vertices[(edge_index+1)%3]; - EDGE_PLANE(e0,e1,triangle_normal,plane); - } - - //! Gets the relative transformation of this triangle - /*! + btVector3 m_vertices[3]; + + GIM_TRIANGLE() : m_margin(0.1f) + { + } + + SIMD_FORCE_INLINE GIM_AABB get_box() const + { + return GIM_AABB(m_vertices[0], m_vertices[1], m_vertices[2], m_margin); + } + + SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const + { + TRIANGLE_NORMAL(m_vertices[0], m_vertices[1], m_vertices[2], normal); + } + + SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const + { + TRIANGLE_PLANE(m_vertices[0], m_vertices[1], m_vertices[2], plane); + ; + } + + SIMD_FORCE_INLINE void apply_transform(const btTransform &trans) + { + m_vertices[0] = trans(m_vertices[0]); + m_vertices[1] = trans(m_vertices[1]); + m_vertices[2] = trans(m_vertices[2]); + } + + SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index, const btVector3 &triangle_normal, btVector4 &plane) const + { + const btVector3 &e0 = m_vertices[edge_index]; + const btVector3 &e1 = m_vertices[(edge_index + 1) % 3]; + EDGE_PLANE(e0, e1, triangle_normal, plane); + } + + //! Gets the relative transformation of this triangle + /*! The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0: - triangle normal corresponds to Z axis. - 1st normalized edge corresponds to X axis, */ - SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform) const - { - btMatrix3x3 & matrix = triangle_transform.getBasis(); - - btVector3 zaxis; - get_normal(zaxis); - MAT_SET_Z(matrix,zaxis); + SIMD_FORCE_INLINE void get_triangle_transform(btTransform &triangle_transform) const + { + btMatrix3x3 &matrix = triangle_transform.getBasis(); - btVector3 xaxis = m_vertices[1] - m_vertices[0]; - VEC_NORMALIZE(xaxis); - MAT_SET_X(matrix,xaxis); + btVector3 zaxis; + get_normal(zaxis); + MAT_SET_Z(matrix, zaxis); - //y axis - xaxis = zaxis.cross(xaxis); - MAT_SET_Y(matrix,xaxis); + btVector3 xaxis = m_vertices[1] - m_vertices[0]; + VEC_NORMALIZE(xaxis); + MAT_SET_X(matrix, xaxis); - triangle_transform.setOrigin(m_vertices[0]); - } + //y axis + xaxis = zaxis.cross(xaxis); + MAT_SET_Y(matrix, xaxis); + triangle_transform.setOrigin(m_vertices[0]); + } //! Test triangles by finding separating axis /*! @@ -195,8 +189,8 @@ public: \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle */ bool collide_triangle_hard_test( - const GIM_TRIANGLE & other, - GIM_TRIANGLE_CONTACT_DATA & contact_data) const; + const GIM_TRIANGLE &other, + GIM_TRIANGLE_CONTACT_DATA &contact_data) const; //! Test boxes before doing hard test /*! @@ -205,16 +199,16 @@ public: \ */ SIMD_FORCE_INLINE bool collide_triangle( - const GIM_TRIANGLE & other, - GIM_TRIANGLE_CONTACT_DATA & contact_data) const + const GIM_TRIANGLE &other, + GIM_TRIANGLE_CONTACT_DATA &contact_data) const { //test box collisioin - GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin); - GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin); - if(!boxu.has_collision(boxv)) return false; + GIM_AABB boxu(m_vertices[0], m_vertices[1], m_vertices[2], m_margin); + GIM_AABB boxv(other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], other.m_margin); + if (!boxu.has_collision(boxv)) return false; //do hard test - return collide_triangle_hard_test(other,contact_data); + return collide_triangle_hard_test(other, contact_data); } /*! @@ -246,43 +240,43 @@ if 0.0<= u+v <=1.0 then they are inside of triangle \return false if the point is outside of triangle.This function doesn't take the margin */ SIMD_FORCE_INLINE bool get_uv_parameters( - const btVector3 & point, - const btVector3 & tri_plane, - GREAL & u, GREAL & v) const + const btVector3 &point, + const btVector3 &tri_plane, + GREAL &u, GREAL &v) const { - btVector3 _axe1 = m_vertices[1]-m_vertices[0]; - btVector3 _axe2 = m_vertices[2]-m_vertices[0]; + btVector3 _axe1 = m_vertices[1] - m_vertices[0]; + btVector3 _axe2 = m_vertices[2] - m_vertices[0]; btVector3 _vecproj = point - m_vertices[0]; - GUINT _i1 = (tri_plane.closestAxis()+1)%3; - GUINT _i2 = (_i1+1)%3; - if(btFabs(_axe2[_i2])<G_EPSILON) + GUINT _i1 = (tri_plane.closestAxis() + 1) % 3; + GUINT _i2 = (_i1 + 1) % 3; + if (btFabs(_axe2[_i2]) < G_EPSILON) { - u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1] - _axe1[_i1]*_axe2[_i2]); - v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1]; + u = (_vecproj[_i2] * _axe2[_i1] - _vecproj[_i1] * _axe2[_i2]) / (_axe1[_i2] * _axe2[_i1] - _axe1[_i1] * _axe2[_i2]); + v = (_vecproj[_i1] - u * _axe1[_i1]) / _axe2[_i1]; } else { - u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2] - _axe1[_i2]*_axe2[_i1]); - v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2]; + u = (_vecproj[_i1] * _axe2[_i2] - _vecproj[_i2] * _axe2[_i1]) / (_axe1[_i1] * _axe2[_i2] - _axe1[_i2] * _axe2[_i1]); + v = (_vecproj[_i2] - u * _axe1[_i2]) / _axe2[_i2]; } - if(u<-G_EPSILON) + if (u < -G_EPSILON) { return false; } - else if(v<-G_EPSILON) + else if (v < -G_EPSILON) { return false; } else { btScalar sumuv; - sumuv = u+v; - if(sumuv<-G_EPSILON) + sumuv = u + v; + if (sumuv < -G_EPSILON) { return false; } - else if(sumuv-1.0f>G_EPSILON) + else if (sumuv - 1.0f > G_EPSILON) { return false; } @@ -294,50 +288,49 @@ if 0.0<= u+v <=1.0 then they are inside of triangle /*! Test if point is in triangle, with m_margin tolerance */ - SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const + SIMD_FORCE_INLINE bool is_point_inside(const btVector3 &point, const btVector3 &tri_normal) const { //Test with edge 0 btVector4 edge_plane; - this->get_edge_plane(0,tri_normal,edge_plane); - GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point); - if(dist-m_margin>0.0f) return false; // outside plane + this->get_edge_plane(0, tri_normal, edge_plane); + GREAL dist = DISTANCE_PLANE_POINT(edge_plane, point); + if (dist - m_margin > 0.0f) return false; // outside plane - this->get_edge_plane(1,tri_normal,edge_plane); - dist = DISTANCE_PLANE_POINT(edge_plane,point); - if(dist-m_margin>0.0f) return false; // outside plane + this->get_edge_plane(1, tri_normal, edge_plane); + dist = DISTANCE_PLANE_POINT(edge_plane, point); + if (dist - m_margin > 0.0f) return false; // outside plane - this->get_edge_plane(2,tri_normal,edge_plane); - dist = DISTANCE_PLANE_POINT(edge_plane,point); - if(dist-m_margin>0.0f) return false; // outside plane + this->get_edge_plane(2, tri_normal, edge_plane); + dist = DISTANCE_PLANE_POINT(edge_plane, point); + if (dist - m_margin > 0.0f) return false; // outside plane return true; } - //! Bidireccional ray collision SIMD_FORCE_INLINE bool ray_collision( - const btVector3 & vPoint, - const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal, - GREAL & tparam, GREAL tmax = G_REAL_INFINITY) + const btVector3 &vPoint, + const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal, + GREAL &tparam, GREAL tmax = G_REAL_INFINITY) { btVector4 faceplane; { btVector3 dif1 = m_vertices[1] - m_vertices[0]; btVector3 dif2 = m_vertices[2] - m_vertices[0]; - VEC_CROSS(faceplane,dif1,dif2); - faceplane[3] = m_vertices[0].dot(faceplane); + VEC_CROSS(faceplane, dif1, dif2); + faceplane[3] = m_vertices[0].dot(faceplane); } - GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax); - if(res == 0) return false; - if(! is_point_inside(pout,faceplane)) return false; + GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax); + if (res == 0) return false; + if (!is_point_inside(pout, faceplane)) return false; - if(res==2) //invert normal + if (res == 2) //invert normal { - triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]); + triangle_normal.setValue(-faceplane[0], -faceplane[1], -faceplane[2]); } else { - triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); + triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]); } VEC_NORMALIZE(triangle_normal); @@ -345,36 +338,31 @@ if 0.0<= u+v <=1.0 then they are inside of triangle return true; } - //! one direccion ray collision SIMD_FORCE_INLINE bool ray_collision_front_side( - const btVector3 & vPoint, - const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal, - GREAL & tparam, GREAL tmax = G_REAL_INFINITY) + const btVector3 &vPoint, + const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal, + GREAL &tparam, GREAL tmax = G_REAL_INFINITY) { btVector4 faceplane; { btVector3 dif1 = m_vertices[1] - m_vertices[0]; btVector3 dif2 = m_vertices[2] - m_vertices[0]; - VEC_CROSS(faceplane,dif1,dif2); - faceplane[3] = m_vertices[0].dot(faceplane); + VEC_CROSS(faceplane, dif1, dif2); + faceplane[3] = m_vertices[0].dot(faceplane); } - GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax); - if(res != 1) return false; + GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax); + if (res != 1) return false; - if(!is_point_inside(pout,faceplane)) return false; + if (!is_point_inside(pout, faceplane)) return false; - triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); + triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]); VEC_NORMALIZE(triangle_normal); return true; } - }; - - - -#endif // GIM_TRI_COLLISION_H_INCLUDED +#endif // GIM_TRI_COLLISION_H_INCLUDED |