diff --git a/Extras/GIMPACT/include/GIMPACT/Bullet/btBoxCollision.h b/Extras/GIMPACT/include/GIMPACT/Bullet/btBoxCollision.h new file mode 100644 index 000000000..c98526f09 --- /dev/null +++ b/Extras/GIMPACT/include/GIMPACT/Bullet/btBoxCollision.h @@ -0,0 +1,651 @@ +#ifndef BT_BOX_COLLISION_H_INCLUDED +#define BT_BOX_COLLISION_H_INCLUDED + +/*! \file gim_box_collision.h +\author Francisco León Nájera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "LinearMath/btTransform.h" + +/*! \defgroup BOUND_AABB_OPERATIONS +*/ +//! @{ + +///Swap numbers +#define BT_SWAP_NUMBERS(a,b){ \ + a = a+b; \ + b = a-b; \ + a = a-b; \ +}\ + + +#define BT_MAX(a,b) (ab?b:a) + +#define BT_GREATER(x, y) fabsf(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 +{ + BT_CONST_BACK_PLANE = 0, + BT_CONST_COLLIDE_PLANE, + BT_CONST_FRONT_PLANE +}; + +//SIMD_FORCE_INLINE bool test_cross_edge_box( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, const btVector3 & extend, +// int dir_index0, +// int dir_index1 +// int component_index0, +// int component_index1) +//{ +// // dir coords are -z and y +// +// const btScalar dir0 = -edge[dir_index0]; +// const btScalar dir1 = edge[dir_index1]; +// btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1; +// btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1; +// //find minmax +// if(pmin>pmax) +// { +// BT_SWAP_NUMBERS(pmin,pmax); +// } +// //find extends +// const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] + +// extend[component_index1] * absolute_edge[dir_index1]; +// +// if(pmin>rad || -rad>pmax) return false; +// return true; +//} +// +//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, btVector3 & extend) +//{ +// +// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2); +//} +// +// +//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, btVector3 & extend) +//{ +// +// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0); +//} +// +//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis( +// const btVector3 & edge, +// const btVector3 & absolute_edge, +// const btVector3 & pointa, +// const btVector3 & pointb, btVector3 & extend) +//{ +// +// 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_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);\ +}\ + + +//! 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) +{ + 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 +{ +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 + + 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; + + for(i=0;i<3;i++) + { + for(j=0;j<3;j++ ) + { + m_AR[i][j] = 1e-6f + fabsf(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) + { + + 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) + { + m_R1to0 = trans0.getBasis().inverse(); + m_T1to0 = m_R1to0 * (-trans0.getOrigin()); + + m_T1to0 += m_R1to0*trans1.getOrigin(); + m_R1to0 *= trans1.getBasis(); + + calc_absolute_matrix(); + } + + SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const + { + return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(), + m_R1to0[1].dot(point) + m_T1to0.y(), + m_R1to0[2].dot(point) + m_T1to0.z()); + } +}; + + +#define BOX_PLANE_EPSILON 0.000001f + +//! Axis aligned box +ATTRIBUTE_ALIGNED16 (class) btAABB +{ +public: + btVector3 m_min; + btVector3 m_max; + + btAABB() + {} + + + 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_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) + { + 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_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + 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) + { + m_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + SIMD_FORCE_INLINE void invalidate() + { + m_min[0] = SIMD_INFINITY; + m_min[1] = SIMD_INFINITY; + m_min[2] = SIMD_INFINITY; + m_max[0] = -SIMD_INFINITY; + m_max[1] = -SIMD_INFINITY; + m_max[2] = -SIMD_INFINITY; + } + + SIMD_FORCE_INLINE void increment_margin(btScalar margin) + { + m_min[0] -= margin; + m_min[1] -= margin; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + SIMD_FORCE_INLINE void copy_with_margin(const btAABB &other, btScalar margin) + { + m_min[0] = other.m_min[0] - margin; + m_min[1] = other.m_min[1] - margin; + m_min[2] = other.m_min[2] - margin; + + m_max[0] = other.m_max[0] + margin; + m_max[1] = other.m_max[1] + margin; + m_max[2] = other.m_max[2] + margin; + } + + template + SIMD_FORCE_INLINE void calc_from_triangle( + 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_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 + SIMD_FORCE_INLINE void calc_from_triangle_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_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; + m_min[2] -= margin; + m_max[0] += margin; + m_max[1] += margin; + m_max[2] += margin; + } + + //! Apply a transform to an AABB + SIMD_FORCE_INLINE void appy_transform(const btTransform & trans) + { + btVector3 center = (m_max+m_min)*0.5f; + btVector3 extends = m_max - center; + // Compute new center + center = trans(center); + + btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()), + extends.dot(trans.getBasis().getRow(1).absolute()), + extends.dot(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) + { + btVector3 center = (m_max+m_min)*0.5f; + btVector3 extends = m_max - center; + // Compute new center + center = trans.transform(center); + + btVector3 textends(extends.dot(trans.m_R1to0.getRow(0).absolute()), + extends.dot(trans.m_R1to0.getRow(1).absolute()), + extends.dot(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) + { + 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]); + } + + //! Merges a point + template + 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_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 + { + 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 + { + 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]); + } + + + 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]) + { + return false; + } + return true; + } + + /*! \brief Finds the Ray intersection parameter. + \param aabb Aligned box + \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 + { + 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; + btScalar Dy = vorigin[1] - center[1]; + 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; + + + btScalar f = vdir[1] * Dz - vdir[2] * Dy; + 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; + f = vdir[0] * Dy - vdir[1] * Dx; + 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 + { + btVector3 center = (m_max+m_min)*0.5f; + btVector3 extend = m_max-center; + + btScalar _fOrigin = direction.dot(center); + btScalar _fMaximumExtent = extend.dot(direction.absolute()); + vmin = _fOrigin - _fMaximumExtent; + vmax = _fOrigin + _fMaximumExtent; + } + + SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const + { + btScalar _fmin,_fmax; + this->projection_interval(plane,_fmin,_fmax); + + if(plane[3] > _fmax + BOX_PLANE_EPSILON) + { + return BT_CONST_BACK_PLANE; // 0 + } + + if(plane[3]+BOX_PLANE_EPSILON >=_fmin) + { + return BT_CONST_COLLIDE_PLANE; //1 + } + return BT_CONST_FRONT_PLANE;//2 + } + + 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 + { + btAABB tbox = box; + tbox.appy_transform_trans_cache(trans1_to_0); + return has_collision(tbox); + } + + //! 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 + { + + //Taken from OPCODE + btVector3 ea,eb;//extends + btVector3 ca,cb;//extends + get_center_extend(ca,ea); + box.get_center_extend(cb,eb); + + + btVector3 T; + btScalar t,t2; + int i; + + // Class I : A's basis vectors + for(i=0;i<3;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; + } + // Class II : B's basis vectors + 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; + } + // Class III : 9 cross products + if(fulltest) + { + 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++) + { + 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; + } + } + } + return true; + } + + //! Simple test for planes. + SIMD_FORCE_INLINE bool collide_plane( + const btVector4 & plane) const + { + eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane); + return (classify == BT_CONST_COLLIDE_PLANE); + } + + //! 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 + { + if(!collide_plane(triangle_plane)) return false; + + btVector3 center,extends; + this->get_center_extend(center,extends); + + const btVector3 v1(p1 - center); + const btVector3 v2(p2 - center); + const btVector3 v3(p3 - center); + + //First axis + 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 With Y axis + 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); + + + 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 With Y axis + 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); + + 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 With Y axis + 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); + + return true; + } +}; + + +//! Compairison of transformation objects +SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) +{ + 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; + return true; +} + + +//! @} + +#endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/Extras/GIMPACT/include/GIMPACT/Bullet/btClipPolygon.h b/Extras/GIMPACT/include/GIMPACT/Bullet/btClipPolygon.h new file mode 100644 index 000000000..32d470a23 --- /dev/null +++ b/Extras/GIMPACT/include/GIMPACT/Bullet/btClipPolygon.h @@ -0,0 +1,186 @@ +#ifndef BT_CLIP_POLYGON_H_INCLUDED +#define BT_CLIP_POLYGON_H_INCLUDED + +/*! \file btClipPolygon.h +\author Francisco León Nájera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "LinearMath/btTransform.h" +#include "LinearMath/btGeometryUtil.h" + +/*! \addtogroup GEOMETRIC_OPERATIONS +*/ +//! @{ + +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) +{ + 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) +{ + 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); + clipped_count++; + } + 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) +{ + int clipped_count = 0; + + + //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;iSIMD_EPSILON)) + { + clipped[clipped_count] = point0; + clipped_count++; + } + + // point 1 + btScalar olddist = firstdist; + btScalar dist = bt_distance_point_plane(plane,point1); + + bt_plane_clip_polygon_collect( + point0,point1, + olddist, + dist, + clipped, + clipped_count); + + olddist = dist; + + + // point 2 + dist = bt_distance_point_plane(plane,point2); + + bt_plane_clip_polygon_collect( + 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); + + return clipped_count; +} + + + + +//! @} + +#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/Extras/GIMPACT/include/GIMPACT/Bullet/btContactProcessing.h b/Extras/GIMPACT/include/GIMPACT/Bullet/btContactProcessing.h new file mode 100644 index 000000000..035b4f1f9 --- /dev/null +++ b/Extras/GIMPACT/include/GIMPACT/Bullet/btContactProcessing.h @@ -0,0 +1,149 @@ +#ifndef BT_CONTACT_H_INCLUDED +#define BT_CONTACT_H_INCLUDED + +/*! \file gim_contact.h +\author Francisco León Nájera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "LinearMath/btTransform.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "GIMPACT/Bullet/btTriangleShapeEx.h" + + +/*! \defgroup CONTACTS +\brief +Functions for managing and sorting contacts resulting from a collision query. +*/ +//! @{ + +/** +Configuration var for applying interpolation of contact normals +*/ +#define NORMAL_CONTACT_AVERAGE 1 + +#define CONTACT_DIFF_EPSILON 0.00001f + +/// Structure for collision results +class BT_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 +public: + BT_CONTACT() + { + } + + BT_CONTACT(const BT_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) + { + } + + BT_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; + unsigned int *_uitmp = (unsigned int *)(&_coords[0]); + _hash = *_uitmp; + _uitmp++; + _hash += (*_uitmp)<<4; + _uitmp++; + _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 +{ +public: + btContactArray() + { + reserve(64); + } + + SIMD_FORCE_INLINE void push_contact( + const btVector3 &point,const btVector3 & normal, + btScalar depth, int feature1, int feature2) + { + push_back( BT_CONTACT(point,normal,depth,feature1,feature2) ); + } + + SIMD_FORCE_INLINE void push_triangle_contacts( + const BT_TRIANGLE_CONTACT & tricontact, + int feature1,int feature2) + { + for(int i = 0;i +{ +public: + btPairSet() + { + reserve(32); + } + inline void push_pair(int index1,int index2) + { + push_back(BT_PAIR(index1,index2)); + } + + inline void push_pair_inv(int index1,int index2) + { + push_back(BT_PAIR(index2,index1)); + } +}; + + + +struct BT_BVH_DATA +{ + btAABB m_bound; + int m_data; +}; + +//! Node Structure for trees +class BT_BVH_TREE_NODE +{ +public: + btAABB m_bound; +protected: + int m_escapeIndexOrDataIndex; +public: + BT_BVH_TREE_NODE() + { + m_escapeIndexOrDataIndex = 0; + } + + SIMD_FORCE_INLINE bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrDataIndex>=0); + } + + SIMD_FORCE_INLINE int getEscapeIndex() const + { + //btAssert(m_escapeIndexOrDataIndex < 0); + return -m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setEscapeIndex(int index) + { + m_escapeIndexOrDataIndex = -index; + } + + SIMD_FORCE_INLINE int getDataIndex() const + { + //btAssert(m_escapeIndexOrDataIndex >= 0); + + return m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setDataIndex(int index) + { + m_escapeIndexOrDataIndex = index; + } + +}; + + +class BT_BVH_DATA_ARRAY:public btAlignedObjectArray +{ +}; + + +class BT_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray +{ +}; + + + + +//! Basic Box tree structure +class btBvhTree +{ +protected: + int m_num_nodes; + BT_BVH_TREE_NODE_ARRAY m_node_array; +protected: + int _sort_and_calc_splitting_index( + BT_BVH_DATA_ARRAY & primitive_boxes, + int startIndex, int endIndex, int splitAxis); + + int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + + void _build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); +public: + btBvhTree() + { + m_num_nodes = 0; + } + + //! prototype functions for box tree management + //!@{ + void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes); + + SIMD_FORCE_INLINE void clearNodes() + { + m_node_array.clear(); + m_num_nodes = 0; + } + + //! node count + SIMD_FORCE_INLINE int getNodeCount() const + { + return m_num_nodes; + } + + //! tells if the node is a leaf + SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const + { + return m_node_array[nodeindex].isLeafNode(); + } + + SIMD_FORCE_INLINE int getNodeData(int nodeindex) const + { + return m_node_array[nodeindex].getDataIndex(); + } + + 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) + { + m_node_array[nodeindex].m_bound = bound; + } + + SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const + { + 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(); + } + + SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const + { + return m_node_array[nodeindex].getEscapeIndex(); + } + + SIMD_FORCE_INLINE const BT_BVH_TREE_NODE * get_node_pointer(int index = 0) const + { + return &m_node_array[index]; + } + + //!@} +}; + + +//! Prototype Base class for primitive classification +/*! +This class is a wrapper for primitive collections. +This tells relevant info for the Bounding Box set classes, which take care of space classification. +This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons. +*/ +class btPrimitiveManagerBase +{ +public: + + //! 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; + //! retrieves only the points of the triangle, and the collision margin + 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. +Requires a Primitive prototype (like btPrimitiveManagerBase ) +*/ +class btGImpactBvh +{ +protected: + btBvhTree m_box_tree; + btPrimitiveManagerBase * m_primitive_manager; + +protected: + //stackless refit + void refit(); +public: + + //! this constructor doesn't build the tree. you must call buildSet + btGImpactBvh() + { + m_primitive_manager = NULL; + } + + //! this constructor doesn't build the tree. you must call buildSet + btGImpactBvh(btPrimitiveManagerBase * primitive_manager) + { + m_primitive_manager = primitive_manager; + } + + SIMD_FORCE_INLINE btAABB getGlobalBox() const + { + btAABB totalbox; + getNodeBound(0, totalbox); + return totalbox; + } + + SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) + { + m_primitive_manager = primitive_manager; + } + + SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const + { + return m_primitive_manager; + } + + +//! node manager prototype functions +///@{ + + //! this attemps to refit the box set. + SIMD_FORCE_INLINE void update() + { + refit(); + } + + //! this rebuild the entire set + void buildSet(); + + //! returns the indices of the primitives in the m_primitive_manager + bool boxQuery(const btAABB & box, btAlignedObjectArray & 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 & collided_results) const + { + btAABB transbox=box; + transbox.appy_transform(transform); + 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 & collided_results) const; + + //! tells if this set has hierarcht + SIMD_FORCE_INLINE bool hasHierarchy() const + { + return true; + } + + //! tells if this set is a trimesh + SIMD_FORCE_INLINE bool isTrimesh() const + { + return m_primitive_manager->is_trimesh(); + } + + //! node count + SIMD_FORCE_INLINE int getNodeCount() const + { + return m_box_tree.getNodeCount(); + } + + //! tells if the node is a leaf + SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const + { + return m_box_tree.isLeafNode(nodeindex); + } + + SIMD_FORCE_INLINE int getNodeData(int nodeindex) const + { + return m_box_tree.getNodeData(nodeindex); + } + + 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) + { + m_box_tree.setNodeBound(nodeindex, bound); + } + + + SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const + { + return m_box_tree.getLeftNode(nodeindex); + } + + SIMD_FORCE_INLINE int getRightNode(int nodeindex) const + { + return m_box_tree.getRightNode(nodeindex); + } + + SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const + { + return m_box_tree.getEscapeNodeIndex(nodeindex); + } + + SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const + { + m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); + } + + + SIMD_FORCE_INLINE const BT_BVH_TREE_NODE * get_node_pointer(int index = 0) const + { + return m_box_tree.get_node_pointer(index); + } + +//! @} + + static float getAverageTreeCollisionTime(); + + + static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1, + btGImpactBvh * boxset2, const btTransform & trans2, + btPairSet & collision_pairs); +}; + + +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h index c52259e0e..03e229b43 100755 --- a/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h +++ b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h @@ -2,32 +2,23 @@ \author Francisco León Nájera */ /* ------------------------------------------------------------------------------ This source file is part of GIMPACT Library. For the latest info, see http://gimpact.sourceforge.net/ -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. email: projectileman@yahoo.com - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: ------------------------------------------------------------------------------ +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. */ #ifndef BVH_CONCAVE_COLLISION_ALGORITHM_H @@ -47,9 +38,8 @@ class btDispatcher; #include "GIMPACT/Bullet/btGImpactShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" - +#include "LinearMath/btIDebugDraw.h" @@ -65,7 +55,7 @@ class btGImpactCollisionAlgorithm : public btCollisionAlgorithm { protected: btCollisionAlgorithm * m_convex_algorithm; - btPersistentManifold* m_manifoldPtr; + btPersistentManifold * m_manifoldPtr; btManifoldResult* m_resultOut; const btDispatcherInfo * m_dispatchInfo; int m_triface0; @@ -73,10 +63,11 @@ protected: int m_triface1; int m_part1; + + //! Creates a new contact point SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(btCollisionObject* body0,btCollisionObject* body1) { - clearCache(); - m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); return m_manifoldPtr; } @@ -90,14 +81,16 @@ protected: } } + SIMD_FORCE_INLINE void destroyContactManifolds() + { + if(m_manifoldPtr == NULL) return; + m_dispatcher->releaseManifold(m_manifoldPtr); + m_manifoldPtr = NULL; + } + SIMD_FORCE_INLINE void clearCache() { - if(m_manifoldPtr) - { - //m_manifoldPtr->clearManifold(); - m_dispatcher->releaseManifold(m_manifoldPtr); - m_manifoldPtr = NULL; - } + destroyContactManifolds(); destroyConvexAlgorithm(); m_triface0 = -1; @@ -106,31 +99,30 @@ protected: m_part1 = -1; } + SIMD_FORCE_INLINE btPersistentManifold* getLastManifold() + { + return m_manifoldPtr; + } + // Call before process collision SIMD_FORCE_INLINE void checkManifold(btCollisionObject* body0,btCollisionObject* body1) { - if(m_manifoldPtr == NULL) + if(getLastManifold() == 0) { newContactManifold(body0,body1); } - /*else if(m_manifoldPtr->getBody0()!=body0) - { - clearCache(); - newContactManifold(body0,body1); - }*/ - m_resultOut->setPersistentManifold(m_manifoldPtr); + + m_resultOut->setPersistentManifold(getLastManifold()); } // Call before process collision SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(btCollisionObject* body0,btCollisionObject* body1) { checkManifold(body0,body1); - /*btConvexConvexAlgorithm::CreateFunc convexcreatefunc; - btCollisionAlgorithmConstructionInfo cinfo; - cinfo.m_dispatcher = m_dispatcher; - cinfo.m_manifold = m_manifoldPtr;*/ - btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(body0,body1,m_manifoldPtr); + + btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm( + body0,body1,getLastManifold()); return convex_algorithm ; } @@ -144,33 +136,63 @@ protected: - SIMD_FORCE_INLINE void addContactPoint(btCollisionObject * body0, + void addContactPoint(btCollisionObject * body0, btCollisionObject * body1, const btVector3 & point, const btVector3 & normal, - btScalar distance) - { - checkManifold(body0,body1); - m_resultOut->addContactPoint(normal,point,distance); - } + btScalar distance); - void gimpactcompound_vs_gimpactcompound_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - btGImpactCompoundShape * shape0, - btGImpactCompoundShape * shape1,gim_pair_set & pairset) const; +//! Collision routines +//!@{ - void gimpacttrimeshpart_vs_gimpacttrimeshpart_find_pairs( - const btTransform & trans0, - const btTransform & trans1, + void collide_gjk_triangles(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactMeshShapePart * shape0, + btGImpactMeshShapePart * shape1, + const int * pairs, int pair_count); + + void collide_sat_triangles(btCollisionObject * body0, + btCollisionObject * body1, btGImpactMeshShapePart * shape0, - btGImpactMeshShapePart * shape1,gim_pair_set & pairset) const; + btGImpactMeshShapePart * shape1, + const int * pairs, int pair_count); - void gimpactcompound_vs_gimpacttrimeshpart_find_pairs( + + + + void shape_vs_shape_collision( + btCollisionObject * body0, + btCollisionObject * body1, + btCollisionShape * shape0, + btCollisionShape * shape1); + + void convex_vs_convex_collision(btCollisionObject * body0, + btCollisionObject * body1, + btCollisionShape * shape0, + btCollisionShape * shape1); + + + + void gimpact_vs_gimpact_find_pairs( const btTransform & trans0, const btTransform & trans1, - btGImpactCompoundShape * shape0, - btGImpactMeshShapePart * shape1,gim_pair_set & pairset) const; + btGImpactShapeInterface * shape0, + btGImpactShapeInterface * shape1,btPairSet & pairset); + + void gimpact_vs_shape_find_pairs( + const btTransform & trans0, + const btTransform & trans1, + btGImpactShapeInterface * shape0, + btCollisionShape * shape1, + btAlignedObjectArray & collided_primitives); + + + void gimpacttrimeshpart_vs_plane_collision( + btCollisionObject * body0, + btCollisionObject * body1, + btGImpactMeshShapePart * shape0, + btStaticPlaneShape * shape1,bool swapped); + public: @@ -197,92 +219,27 @@ public: //! Use this function for register the algorithm externally static void registerAlgorithm(btCollisionDispatcher * dispatcher); - //! Collision algorithms - //!@{ + //! Gets the average time in miliseconds of tree collisions + static float getAverageTreeCollisionTime(); + + //! Gets the average time in miliseconds of triangle collisions + static float getAverageTriangleCollisionTime(); + //! Collides two gimpact shapes + /*! + \pre shape0 and shape1 couldn't be btGImpactMeshShape objects + */ - - void shape_vs_shape_collision( - btCollisionObject * body0, + void gimpact_vs_gimpact(btCollisionObject * body0, btCollisionObject * body1, - btCollisionShape * shape0, - btCollisionShape * shape1,bool swapped); + btGImpactShapeInterface * shape0, + btGImpactShapeInterface * shape1); - void convex_vs_convex_collision(btCollisionObject * body0, + void gimpact_vs_shape(btCollisionObject * body0, btCollisionObject * body1, - btCollisionShape * shape0, - btCollisionShape * shape1); - - void gimpacttrimesh_vs_shape_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShape * shape0, - btCollisionShape * shape1,bool swapped); - - void gimpacttrimesh_vs_gimpacttrimesh( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShape * shape0, - btGImpactMeshShape * shape1); - - void gimpacttrimesh_vs_gimpactcompound( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShape * shape0, - btGImpactCompoundShape * shape1,bool swapped); - - void gimpacttrimesh_vs_trimeshpart( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShape * shape0, - btGImpactMeshShapePart * shape1,bool swapped); - - - void gimpactcompound_vs_gimpactcompound_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactCompoundShape * shape0, - btGImpactCompoundShape * shape1); - - - void gimpactcompound_vs_gimpacttrimeshpart_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactCompoundShape * shape0, - btGImpactMeshShapePart * shape1,bool swapped); - - - void gimpactcompound_vs_shape_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactCompoundShape * shape0, - btCollisionShape * shape1,bool swapped); - - void gimpacttrimeshpart_vs_gimpacttrimeshpart_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShapePart * shape0, - btGImpactMeshShapePart * shape1,bool swapped); - - void gimpacttrimeshpart_vs_plane_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShapePart * shape0, - btStaticPlaneShape * shape1,bool swapped); - - - void gimpacttrimeshpart_vs_concave_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShapePart * shape0, - btConcaveShape * shape1,bool swapped); - - void gimpacttrimeshpart_vs_shape_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShapePart * shape0, + btGImpactShapeInterface * shape0, btCollisionShape * shape1,bool swapped); void gimpact_vs_compoundshape(btCollisionObject * body0, @@ -290,21 +247,26 @@ public: btGImpactShapeInterface * shape0, btCompoundShape * shape1,bool swapped); - - void gimpact_vs_shape(btCollisionObject * body0, + void gimpact_vs_concave( + btCollisionObject * body0, btCollisionObject * body1, btGImpactShapeInterface * shape0, - btCollisionShape * shape1,bool swapped); + btConcaveShape * shape1,bool swapped); + + + + +//!@} - void gimpact_vs_gimpact(btCollisionObject * body0, - btCollisionObject * body1, - btGImpactShapeInterface * shape0, - btGImpactShapeInterface * shape1); - //!@} }; +//algorithm details +//#define BULLET_TRIANGLE_COLLISION 1 +#define GIMPACT_VS_PLANE_COLLISION 1 + + #endif //BVH_CONCAVE_COLLISION_ALGORITHM_H diff --git a/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactMassUtil.h b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactMassUtil.h index 6f59377c3..7d1c26bc8 100755 --- a/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactMassUtil.h +++ b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactMassUtil.h @@ -2,32 +2,23 @@ \author Francisco León Nájera */ /* ------------------------------------------------------------------------------ This source file is part of GIMPACT Library. For the latest info, see http://gimpact.sourceforge.net/ -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. email: projectileman@yahoo.com - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: ------------------------------------------------------------------------------ +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. */ diff --git a/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactQuantizedBvh.h b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactQuantizedBvh.h new file mode 100644 index 000000000..610862b4d --- /dev/null +++ b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactQuantizedBvh.h @@ -0,0 +1,379 @@ +#ifndef GIM_QUANTIZED_SET_H_INCLUDED +#define GIM_QUANTIZED_SET_H_INCLUDED + +/*! \file btGImpactQuantizedBvh.h +\author Francisco León Nájera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "GIMPACT/Bullet/btGImpactBvh.h" +#include "GIMPACT/Bullet/btQuantization.h" + + + +/*! \defgroup BOX_TREES + + + +*/ +//! @{ + + +///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 +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrDataIndex; + + BT_QUANTIZED_BVH_NODE() + { + m_escapeIndexOrDataIndex = 0; + } + + SIMD_FORCE_INLINE bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrDataIndex>=0); + } + + SIMD_FORCE_INLINE int getEscapeIndex() const + { + //btAssert(m_escapeIndexOrDataIndex < 0); + return -m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setEscapeIndex(int index) + { + m_escapeIndexOrDataIndex = -index; + } + + SIMD_FORCE_INLINE int getDataIndex() const + { + //btAssert(m_escapeIndexOrDataIndex >= 0); + + return m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setDataIndex(int index) + { + m_escapeIndexOrDataIndex = index; + } + + SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( + 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]) + { + return false; + } + return true; + } + +}; + + + +class BT_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray +{ +}; + + + + +//! Basic Box tree structure +class btQuantizedBvhTree +{ +protected: + int m_num_nodes; + BT_QUANTIZED_BVH_NODE_ARRAY m_node_array; + btAABB m_global_bound; + btVector3 m_bvhQuantization; +protected: + void calc_quantization(BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) ); + + int _sort_and_calc_splitting_index( + BT_BVH_DATA_ARRAY & primitive_boxes, + int startIndex, int endIndex, int splitAxis); + + int _calc_splitting_axis(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); + + void _build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); +public: + btQuantizedBvhTree() + { + m_num_nodes = 0; + } + + //! prototype functions for box tree management + //!@{ + void build_tree(BT_BVH_DATA_ARRAY & primitive_boxes); + + SIMD_FORCE_INLINE void quantizePoint( + unsigned short * quantizedpoint, const btVector3 & point) const + { + 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 + { + return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax); + } + + SIMD_FORCE_INLINE void clearNodes() + { + m_node_array.clear(); + m_num_nodes = 0; + } + + //! node count + SIMD_FORCE_INLINE int getNodeCount() const + { + return m_num_nodes; + } + + //! tells if the node is a leaf + SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const + { + return m_node_array[nodeindex].isLeafNode(); + } + + SIMD_FORCE_INLINE int getNodeData(int nodeindex) const + { + return m_node_array[nodeindex].getDataIndex(); + } + + 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); + + bound.m_max = bt_unquantize( + m_node_array[nodeindex].m_quantizedAabbMax, + m_global_bound.m_min,m_bvhQuantization); + } + + 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_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; + } + + 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(); + } + + SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const + { + return m_node_array[nodeindex].getEscapeIndex(); + } + + SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const + { + return &m_node_array[index]; + } + + //!@} +}; + + + +//! Structure for containing Boxes +/*! +This class offers an structure for managing a box tree of primitives. +Requires a Primitive prototype (like btPrimitiveManagerBase ) +*/ +class btGImpactQuantizedBvh +{ +protected: + btQuantizedBvhTree m_box_tree; + btPrimitiveManagerBase * m_primitive_manager; + +protected: + //stackless refit + void refit(); +public: + + //! this constructor doesn't build the tree. you must call buildSet + btGImpactQuantizedBvh() + { + m_primitive_manager = NULL; + } + + //! this constructor doesn't build the tree. you must call buildSet + btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager) + { + m_primitive_manager = primitive_manager; + } + + SIMD_FORCE_INLINE btAABB getGlobalBox() const + { + btAABB totalbox; + getNodeBound(0, totalbox); + return totalbox; + } + + SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) + { + m_primitive_manager = primitive_manager; + } + + SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const + { + return m_primitive_manager; + } + + +//! node manager prototype functions +///@{ + + //! this attemps to refit the box set. + SIMD_FORCE_INLINE void update() + { + refit(); + } + + //! this rebuild the entire set + void buildSet(); + + //! returns the indices of the primitives in the m_primitive_manager + bool boxQuery(const btAABB & box, btAlignedObjectArray & 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 & collided_results) const + { + btAABB transbox=box; + transbox.appy_transform(transform); + 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 & collided_results) const; + + //! tells if this set has hierarcht + SIMD_FORCE_INLINE bool hasHierarchy() const + { + return true; + } + + //! tells if this set is a trimesh + SIMD_FORCE_INLINE bool isTrimesh() const + { + return m_primitive_manager->is_trimesh(); + } + + //! node count + SIMD_FORCE_INLINE int getNodeCount() const + { + return m_box_tree.getNodeCount(); + } + + //! tells if the node is a leaf + SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const + { + return m_box_tree.isLeafNode(nodeindex); + } + + SIMD_FORCE_INLINE int getNodeData(int nodeindex) const + { + return m_box_tree.getNodeData(nodeindex); + } + + 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) + { + m_box_tree.setNodeBound(nodeindex, bound); + } + + + SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const + { + return m_box_tree.getLeftNode(nodeindex); + } + + SIMD_FORCE_INLINE int getRightNode(int nodeindex) const + { + return m_box_tree.getRightNode(nodeindex); + } + + SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const + { + return m_box_tree.getEscapeNodeIndex(nodeindex); + } + + SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const + { + m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); + } + + + SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const + { + return m_box_tree.get_node_pointer(index); + } + +//! @} + + static float getAverageTreeCollisionTime(); + + + static void find_collision(btGImpactQuantizedBvh * boxset1, const btTransform & trans1, + btGImpactQuantizedBvh * boxset2, const btTransform & trans2, + btPairSet & collision_pairs); +}; + + +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactShape.h b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactShape.h index 076a494df..0d7490461 100755 --- a/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactShape.h +++ b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactShape.h @@ -2,32 +2,23 @@ \author Francisco León Nájera */ /* ------------------------------------------------------------------------------ This source file is part of GIMPACT Library. For the latest info, see http://gimpact.sourceforge.net/ -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. email: projectileman@yahoo.com - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: ------------------------------------------------------------------------------ +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. */ @@ -40,14 +31,18 @@ email: projectileman@yahoo.com #include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "BulletCollision/CollisionDispatch/btCollisionWorld.h" #include "BulletCollision/CollisionShapes/btConcaveShape.h" +#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btAlignedObjectArray.h" -#include "GIMPACT/core/gim_box_set.h" +#include "GIMPACT/Bullet/btGImpactQuantizedBvh.h" // box tree class +//! declare Quantized trees, (you can change to float based trees) +typedef btGImpactQuantizedBvh btGImpactBoxSet; + enum eGIMPACT_SHAPE_TYPE { CONST_GIMPACT_COMPOUND_SHAPE = 0, @@ -55,16 +50,56 @@ enum eGIMPACT_SHAPE_TYPE CONST_GIMPACT_TRIMESH_SHAPE }; + +//! Helper class for tetrahedrons +class btTetrahedronShapeEx:public btBU_Simplex1to4 +{ +public: + btTetrahedronShapeEx() + { + m_numVertices = 4; + } + + + SIMD_FORCE_INLINE void setVertices( + const btVector3 & v0,const btVector3 & v1, + const btVector3 & v2,const btVector3 & v3) + { + m_vertices[0] = v0; + m_vertices[1] = v1; + m_vertices[2] = v2; + m_vertices[3] = v3; + recalcLocalAabb(); + } +}; + + //! Base class for gimpact shapes class btGImpactShapeInterface : public btConcaveShape { protected: - GIM_AABB m_localAABB; + btAABB m_localAABB; bool m_needs_update; btVector3 localScaling; + btGImpactBoxSet m_box_set;// optionally boxset //! use this function for perfofm refit in bounding boxes - virtual void calcLocalAABB() = 0; + //! 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(); + } public: @@ -96,7 +131,7 @@ public: */ void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { - GIM_AABB transformedbox = m_localAABB; + btAABB transformedbox = m_localAABB; transformedbox.appy_transform(t); aabbMin = transformedbox.m_min; aabbMax = transformedbox.m_max; @@ -109,7 +144,7 @@ public: } //! Obtains the local box, which is the global calculated box of the total of subshapes - const GIM_AABB & getLocalBox() + SIMD_FORCE_INLINE const btAABB & getLocalBox() { return m_localAABB; } @@ -120,13 +155,7 @@ public: return GIMPACT_SHAPE_PROXYTYPE; } - //! Base method for determinig which kind of GIMPACT shape we get - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() = 0; - - //! Determines if this class has a hierarchy structure for sorting its primitives - virtual bool hasBoxSet() const = 0; - - /*! + /*! \post You must call updateBound() for update the box set. */ virtual void setLocalScaling(const btVector3& scaling) @@ -134,12 +163,115 @@ public: localScaling = scaling; postUpdate(); } + virtual const btVector3& getLocalScaling() const { return localScaling; } + virtual void setMargin(btScalar margin) + { + 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() = 0; + + //! gets boxset + SIMD_FORCE_INLINE btGImpactBoxSet * getBoxSet() + { + return &m_box_set; + } + + //! Determines if this class has a hierarchy structure for sorting its primitives + SIMD_FORCE_INLINE bool hasBoxSet() const + { + if(m_box_set.getNodeCount() == 0) return false; + return true; + } + + //! Obtains the primitive manager + virtual const btPrimitiveManagerBase * getPrimitiveManager() const = 0; + + + //! Gets the number of children + virtual int getNumChildShapes() const = 0; + + //! if true, then its children must get transforms. + virtual bool childrenHasTransform() const = 0; + + //! Determines if this shape has triangles + virtual bool needsRetrieveTriangles() const = 0; + + //! 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; + + + + //! call when reading child shapes + virtual void lockChildShapes() const + { + } + + virtual void unlockChildShapes() const + { + } + + //! if this trimesh + SIMD_FORCE_INLINE void getPrimitiveTriangle(int index,btPrimitiveTriangle & triangle) const + { + 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; + } + + //! 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; + + //! Sets the children transform + /*! + \post You must call updateBound() for update the box set. + */ + 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 { @@ -155,23 +287,23 @@ public: //!@} - }; -//! btGIMPACTCompoundShape allows to handle multiple btCollisionShape objects at once +//! btGImpactCompoundShape allows to handle multiple btCollisionShape objects at once /*! -This allows for concave collision objects. This is more general then the Static Concave btTriangleMeshShape. +This class only can manage Convex subshapes */ class btGImpactCompoundShape : public btGImpactShapeInterface { public: //! compound primitive manager - class CompoundPrimitiveManager + class CompoundPrimitiveManager:public btPrimitiveManagerBase { public: btGImpactCompoundShape * m_compoundShape; + CompoundPrimitiveManager(const CompoundPrimitiveManager& compound) { m_compoundShape = compound.m_compoundShape; @@ -187,124 +319,175 @@ public: m_compoundShape = NULL; } - SIMD_FORCE_INLINE bool is_trimesh() const + virtual bool is_trimesh() const { return false; } - SIMD_FORCE_INLINE GUINT get_primitive_count() const + virtual int get_primitive_count() const { - return (GUINT )m_compoundShape->getNumChildShapes(); + return (int )m_compoundShape->getNumChildShapes(); } - SIMD_FORCE_INLINE void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) const + virtual void get_primitive_box(int prim_index ,btAABB & primbox) const { - btTransform prim_trans = m_compoundShape->getChildTransform(prim_index); + btTransform prim_trans; + if(m_compoundShape->childrenHasTransform()) + { + prim_trans = m_compoundShape->getChildTransform(prim_index); + } + else + { + prim_trans.setIdentity(); + } const btCollisionShape* shape = m_compoundShape->getChildShape(prim_index); shape->getAabb(prim_trans,primbox.m_min,primbox.m_max); } - SIMD_FORCE_INLINE void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) const + virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const { btAssert(0); } }; - class BoxSetClass: public GIM_BOX_TREE_SET - { - public: - }; protected: - - BoxSetClass m_box_set; + CompoundPrimitiveManager m_primitive_manager; btAlignedObjectArray m_childTransforms; btAlignedObjectArray m_childShapes; - //! use this function for perfofm refit in bounding boxes - virtual void calcLocalAABB() - { - if(m_box_set.getNodeCount() == 0) - { - m_box_set.buildSet(); - } - else - { - m_box_set.update(); - } - - m_localAABB = m_box_set.getGlobalBox(); - } public: - btGImpactCompoundShape() + btGImpactCompoundShape(bool children_has_transform = true) { - m_box_set.setPrimitiveManager(CompoundPrimitiveManager(this)); + m_primitive_manager.m_compoundShape = this; + m_box_set.setPrimitiveManager(&m_primitive_manager); } virtual ~btGImpactCompoundShape() { } - - //! Obtains the primitive manager - SIMD_FORCE_INLINE const CompoundPrimitiveManager & getPrimitiveManager() const + + + //! if true, then its children must get transforms. + virtual bool childrenHasTransform() const { - return m_box_set.getPrimitiveManager(); + if(m_childTransforms.size()==0) return false; + return true; } - //! Use this method for adding children + + //! Obtains the primitive manager + virtual const btPrimitiveManagerBase * getPrimitiveManager() const + { + return &m_primitive_manager; + } + + //! Obtains the compopund primitive manager + SIMD_FORCE_INLINE CompoundPrimitiveManager * getCompoundPrimitiveManager() + { + return &m_primitive_manager; + } + + //! Gets the number of children + 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) { + btAssert(shape->isConvex()); m_childTransforms.push_back(localTransform); m_childShapes.push_back(shape); } - - //! Gets the number of children - int getNumChildShapes() const + + //! Use this method for adding children. Only Convex shapes are allowed. + void addChildShape(btCollisionShape* shape) { - return int (m_childShapes.size()); + btAssert(shape->isConvex()); + m_childShapes.push_back(shape); } //! Gets the children - btCollisionShape* getChildShape(int index) + virtual btCollisionShape* getChildShape(int index) { return m_childShapes[index]; } //! Gets the children - const btCollisionShape* getChildShape(int index) const + virtual const btCollisionShape* getChildShape(int index) const { return m_childShapes[index]; } + //! 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); + } + } + + //! Gets the children transform - btTransform getChildTransform(int index) const + virtual btTransform getChildTransform(int index) const { + btAssert(m_childTransforms.size() == m_childShapes.size()); return m_childTransforms[index]; } //! Sets the children transform - /*! - \post You must call updateBound() for update the box set. + /*! + \post You must call updateBound() for update the box set. */ - 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; postUpdate(); } + //! Determines if this shape has triangles + virtual bool needsRetrieveTriangles() const + { + return false; + } + + //! Determines if this shape has tetrahedrons + virtual bool needsRetrieveTetrahedrons() const + { + return false; + } + + + virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + { + btAssert(0); + } + + virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + { + btAssert(0); + } + + //! Calculates the exact inertia tensor for this shape virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); - - BoxSetClass * getBoxSet() - { - return &m_box_set; - } - virtual char* getName()const { return "GImpactCompound"; @@ -315,41 +498,8 @@ public: return CONST_GIMPACT_COMPOUND_SHAPE; } - virtual bool hasBoxSet() const - { - if(m_box_set.getNodeCount() == 0) return false; - return true; - } - - virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const; - - }; -//! Helper class for colliding Bullet Triangle Shapes -/*! -This class implements a better getAabb method than the previous btTriangleShape class -*/ -class btTriangleShapeEx: public btTriangleShape -{ -public: - btTriangleShapeEx(const btVector3& p0,const btVector3& p1,const btVector3& p2): btTriangleShape(p0,p1,p2) - { - } - - 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]); - - GIM_AABB trianglebox(tv0,tv1,tv2,m_collisionMargin); - aabbMin = trianglebox.m_min; - aabbMax = trianglebox.m_max; - } - - -}; //! This class manages a sub part of a mesh supplied by the btStridingMeshInterface interface. @@ -366,14 +516,14 @@ public: /*! Manages the info from btStridingMeshInterface object and controls the Lock/Unlock mechanism */ - class TrimeshPrimitiveManager + class TrimeshPrimitiveManager:public btPrimitiveManagerBase { public: btScalar m_margin; btStridingMeshInterface * m_meshInterface; btVector3 m_scale; int m_part; - GUINT m_lock_count; + int m_lock_count; const unsigned char *vertexbase; int numverts; PHY_ScalarType type; @@ -387,7 +537,7 @@ public: { m_meshInterface = NULL; m_part = 0; - m_margin = 0.1f; + m_margin = 0.01f; m_scale = btVector3(1.f,1.f,1.f); m_lock_count = 0; vertexbase = 0; @@ -403,7 +553,7 @@ public: m_meshInterface = manager.m_meshInterface; m_part = manager.m_part; m_margin = manager.m_margin; - m_scale = manager.m_meshInterface->getScaling(); + m_scale = manager.m_scale; m_lock_count = 0; vertexbase = 0; numverts = 0; @@ -459,42 +609,42 @@ public: m_lock_count = 0; } - SIMD_FORCE_INLINE bool is_trimesh() const + virtual bool is_trimesh() const { return true; } - SIMD_FORCE_INLINE GUINT get_primitive_count() const + virtual int get_primitive_count() const { - return (GUINT )numfaces; + return (int )numfaces; } - SIMD_FORCE_INLINE GUINT get_vertex_count() const + SIMD_FORCE_INLINE int get_vertex_count() const { - return (GUINT )numverts; + return (int )numverts; } - SIMD_FORCE_INLINE void get_indices(GUINT face_index,GUINT &i0,GUINT &i1,GUINT &i2) const + SIMD_FORCE_INLINE void get_indices(int face_index,int &i0,int &i1,int &i2) const { if(indicestype == PHY_SHORT) { - GUSHORT * s_indices = (GUSHORT *)(indexbase + face_index*indexstride); + short * s_indices = (short *)(indexbase + face_index*indexstride); i0 = s_indices[0]; i1 = s_indices[1]; i2 = s_indices[2]; } else { - GUINT * i_indices = (GUINT *)(indexbase + face_index*indexstride); + int * i_indices = (int *)(indexbase + face_index*indexstride); i0 = i_indices[0]; i1 = i_indices[1]; i2 = i_indices[2]; } } - SIMD_FORCE_INLINE void get_vertex(GUINT vertex_index, btVector3 & vertex) const + SIMD_FORCE_INLINE void get_vertex(int vertex_index, btVector3 & vertex) const { - if(indicestype == PHY_DOUBLE) + if(type == PHY_DOUBLE) { double * dvertices = (double *)(vertexbase + vertex_index*stride); vertex[0] = btScalar(dvertices[0]*m_scale[0]); @@ -510,16 +660,18 @@ public: } } - SIMD_FORCE_INLINE void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) const + virtual void get_primitive_box(int prim_index ,btAABB & primbox) const { - GIM_TRIANGLE triangle; + btPrimitiveTriangle triangle; get_primitive_triangle(prim_index,triangle); - primbox = triangle.get_box(); + primbox.calc_from_triangle_margin( + triangle.m_vertices[0], + triangle.m_vertices[1],triangle.m_vertices[2],triangle.m_margin); } - SIMD_FORCE_INLINE void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) const + virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const { - GUINT indices[3]; + 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]); @@ -527,9 +679,9 @@ public: triangle.m_margin = m_margin; } - SIMD_FORCE_INLINE void get_bullet_triangle(GUINT prim_index,btTriangleShapeEx & triangle) const + SIMD_FORCE_INLINE void get_bullet_triangle(int prim_index,btTriangleShapeEx & triangle) const { - GUINT indices[3]; + 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]); @@ -539,77 +691,109 @@ public: }; - class BoxSetClass: public GIM_BOX_TREE_SET - { - public: - }; - protected: - BoxSetClass m_box_set; - - //! use this function for perfofm refit in bounding boxes - virtual void calcLocalAABB() - { - lock(); - if(m_box_set.getNodeCount() == 0) - { - m_box_set.buildSet(); - } - else - { - m_box_set.update(); - } - unlock(); - - m_localAABB = m_box_set.getGlobalBox(); - } + TrimeshPrimitiveManager m_primitive_manager; public: btGImpactMeshShapePart() { - } - - btGImpactMeshShapePart(const btGImpactMeshShapePart & meshpart) - { - m_box_set.setPrimitiveManager(meshpart.getPrimitiveManager()); + m_box_set.setPrimitiveManager(&m_primitive_manager); } btGImpactMeshShapePart(btStridingMeshInterface * meshInterface, int part) { - m_box_set.setPrimitiveManager(TrimeshPrimitiveManager(meshInterface,part)); + m_primitive_manager.m_meshInterface = meshInterface; + m_primitive_manager.m_part = part; + m_box_set.setPrimitiveManager(&m_primitive_manager); } virtual ~btGImpactMeshShapePart() { } - SIMD_FORCE_INLINE const TrimeshPrimitiveManager & getPrimitiveManager() const + //! if true, then its children must get transforms. + virtual bool childrenHasTransform() const { - return m_box_set.getPrimitiveManager(); + return false; } - SIMD_FORCE_INLINE void lock() const + + //! call when reading child shapes + virtual void lockChildShapes() const { - void * dummy = (void*)(& m_box_set.getPrimitiveManager()); + void * dummy = (void*)(m_box_set.getPrimitiveManager()); TrimeshPrimitiveManager * dummymanager = static_cast(dummy); dummymanager->lock(); } - SIMD_FORCE_INLINE void unlock() const + virtual void unlockChildShapes() const { - void * dummy = (void*)(&m_box_set.getPrimitiveManager()); + void * dummy = (void*)(m_box_set.getPrimitiveManager()); TrimeshPrimitiveManager * dummymanager = static_cast(dummy); dummymanager->unlock(); } + //! Gets the number of children + virtual int getNumChildShapes() const + { + return m_primitive_manager.get_primitive_count(); + } + + + //! Gets the children + virtual btCollisionShape* getChildShape(int index) + { + btAssert(0); + return NULL; + } + + + + //! Gets the child + virtual const btCollisionShape* getChildShape(int index) const + { + btAssert(0); + return NULL; + } + + //! Gets the children transform + virtual btTransform getChildTransform(int index) const + { + btAssert(0); + return btTransform(); + } + + //! Sets the children transform + /*! + \post You must call updateBound() for update the box set. + */ + virtual void setChildTransform(int index, const btTransform & transform) + { + btAssert(0); + } + + + //! Obtains the primitive manager + virtual const btPrimitiveManagerBase * getPrimitiveManager() const + { + return &m_primitive_manager; + } + + SIMD_FORCE_INLINE TrimeshPrimitiveManager * getTrimeshPrimitiveManager() + { + return &m_primitive_manager; + } + + + + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); - SIMD_FORCE_INLINE BoxSetClass * getBoxSet() - { - return &m_box_set; - } + + virtual char* getName()const { @@ -621,89 +805,89 @@ public: return CONST_GIMPACT_TRIMESH_SHAPE_PART; } - virtual bool hasBoxSet() const + //! Determines if this shape has triangles + virtual bool needsRetrieveTriangles() const { - if(m_box_set.getNodeCount() == 0) return false; return true; } - SIMD_FORCE_INLINE GUINT getTriangleCount() const + //! Determines if this shape has tetrahedrons + virtual bool needsRetrieveTetrahedrons() const { - return m_box_set.getPrimitiveManager().get_primitive_count(); + return false; } - SIMD_FORCE_INLINE void getTriangle(GUINT triangle_index, GIM_TRIANGLE & triangle) const + virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const { - m_box_set.getPrimitiveManager().get_primitive_triangle(triangle_index,triangle); + m_primitive_manager.get_bullet_triangle(prim_index,triangle); } - SIMD_FORCE_INLINE void getBulletTriangle(GUINT prim_index,btTriangleShapeEx & triangle) const + virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const { - m_box_set.getPrimitiveManager().get_bullet_triangle(prim_index,triangle); + btAssert(0); } - SIMD_FORCE_INLINE GUINT getVertexCount() const + + + SIMD_FORCE_INLINE int getVertexCount() const { - return m_box_set.getPrimitiveManager().get_vertex_count(); + return m_primitive_manager.get_vertex_count(); } - SIMD_FORCE_INLINE void getVertex(GUINT vertex_index, btVector3 & vertex) const + SIMD_FORCE_INLINE void getVertex(int vertex_index, btVector3 & vertex) const { - m_box_set.getPrimitiveManager().get_vertex(vertex_index,vertex); + m_primitive_manager.get_vertex(vertex_index,vertex); } SIMD_FORCE_INLINE void setMargin(btScalar margin) { - m_box_set.getPrimitiveManager().m_margin = margin; + m_primitive_manager.m_margin = margin; postUpdate(); } SIMD_FORCE_INLINE btScalar getMargin() const { - return m_box_set.getPrimitiveManager().m_margin; + return m_primitive_manager.m_margin; } virtual void setLocalScaling(const btVector3& scaling) { - m_box_set.getPrimitiveManager().m_scale = scaling; + m_primitive_manager.m_scale = scaling; postUpdate(); } virtual const btVector3& getLocalScaling() const { - return m_box_set.getPrimitiveManager().m_scale; + return m_primitive_manager.m_scale; } - SIMD_FORCE_INLINE GUINT getPart() const + SIMD_FORCE_INLINE int getPart() const { - return (GUINT)m_box_set.getPrimitiveManager().m_part; + return (int)m_primitive_manager.m_part; } - virtual void rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; }; //! This class manages a mesh supplied by the btStridingMeshInterface interface. /*! -Set of btGImpactMeshShapePart parts +Set of btGImpactMeshShapePart parts - Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShape, then you must call updateBound() after creating the mesh - You can handle deformable meshes with this shape, by calling postUpdate() every time when changing the mesh vertices. */ - class btGImpactMeshShape : public btGImpactShapeInterface { protected: btAlignedObjectArray m_mesh_parts; void buildMeshParts(btStridingMeshInterface * meshInterface) { - for (int i=0;igetNumSubParts() ;++i ) + for (int i=0;igetNumSubParts() ;++i ) { btGImpactMeshShapePart * newpart = new btGImpactMeshShapePart(meshInterface,i); - m_mesh_parts.push_back(newpart); + m_mesh_parts.push_back(newpart); } } @@ -712,10 +896,10 @@ protected: { m_localAABB.invalidate(); int i = m_mesh_parts.size(); - while(i--) + while(i--) { m_mesh_parts[i]->updateBound(); - m_localAABB.merge(m_mesh_parts[i]->getLocalBox()); + m_localAABB.merge(m_mesh_parts[i]->getLocalBox()); } } @@ -728,14 +912,16 @@ public: virtual ~btGImpactMeshShape() { int i = m_mesh_parts.size(); - while(i--) + while(i--) { btGImpactMeshShapePart * part = m_mesh_parts[i]; - delete part; + delete part; } m_mesh_parts.clear(); } + + int getMeshPartCount() { return m_mesh_parts.size(); @@ -753,45 +939,157 @@ public: return m_mesh_parts[index]; } - + virtual void setLocalScaling(const btVector3& scaling) { localScaling = scaling; int i = m_mesh_parts.size(); - while(i--) + while(i--) { btGImpactMeshShapePart * part = m_mesh_parts[i]; - part->setLocalScaling(scaling); + part->setLocalScaling(scaling); } m_needs_update = true; } + virtual void setMargin(btScalar margin) + { + m_collisionMargin = margin; + + int i = m_mesh_parts.size(); + 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() { int i = m_mesh_parts.size(); - while(i--) + while(i--) { btGImpactMeshShapePart * part = m_mesh_parts[i]; - part->postUpdate(); + part->postUpdate(); } m_needs_update = true; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + + //! Obtains the primitive manager + virtual const btPrimitiveManagerBase * getPrimitiveManager() const + { + btAssert(0); + return NULL; + } + + + //! Gets the number of children + virtual int getNumChildShapes() const + { + btAssert(0); + return 0; + } + + + //! if true, then its children must get transforms. + virtual bool childrenHasTransform() const + { + btAssert(0); + return false; + } + + //! Determines if this shape has triangles + virtual bool needsRetrieveTriangles() const + { + btAssert(0); + return false; + } + + //! Determines if this shape has tetrahedrons + virtual bool needsRetrieveTetrahedrons() const + { + btAssert(0); + return false; + } + + virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const + { + btAssert(0); + } + + virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const + { + btAssert(0); + } + + //! call when reading child shapes + virtual void lockChildShapes() + { + btAssert(0); + } + + virtual void unlockChildShapes() + { + btAssert(0); + } + + + + + //! Retrieves the bound from a child + /*! + */ + virtual void getChildAabb(int child_index,const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + { + btAssert(0); + } + + //! Gets the children + virtual btCollisionShape* getChildShape(int index) + { + btAssert(0); + return NULL; + } + + + //! Gets the child + virtual const btCollisionShape* getChildShape(int index) const + { + btAssert(0); + return NULL; + } + + //! Gets the children transform + virtual btTransform getChildTransform(int index) const + { + btAssert(0); + return btTransform(); + } + + //! Sets the children transform + /*! + \post You must call updateBound() for update the box set. + */ + virtual void setChildTransform(int index, const btTransform & transform) + { + btAssert(0); + } + + virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() { return CONST_GIMPACT_TRIMESH_SHAPE; } - virtual bool hasBoxSet() const - { - return false; - } virtual char* getName()const { diff --git a/Extras/GIMPACT/include/GIMPACT/Bullet/btGeometryOperations.h b/Extras/GIMPACT/include/GIMPACT/Bullet/btGeometryOperations.h new file mode 100644 index 000000000..2c8a0af2b --- /dev/null +++ b/Extras/GIMPACT/include/GIMPACT/Bullet/btGeometryOperations.h @@ -0,0 +1,216 @@ +#ifndef BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED +#define BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED + +/*! \file btGeometryOperations.h +*\author Francisco León Nájera + +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "GIMPACT/Bullet/btBoxCollision.h" + + + +/*! \defgroup GEOMETRIC_OPERATIONS +*/ +//! @{ + + +#define PLANEDIREPSILON 0.0000001f +#define PARALELENORMALS 0.000001f + + +#define BT_CLAMP(number,minval,maxval) (numbermaxval?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) +{ + btVector3 planenormal = (e2-e1).cross(normal); + planenormal.normalize(); + 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 n = e2-e1; + cp = v - e1; + btScalar _scalar = cp.dot(n)/n.dot(n); + if(_scalar <0.0f) + { + cp = e1; + } + else if(_scalar >1.0f) + { + cp = e2; + } + else + { + cp = _scalar*n + e1; + } +} + + +//! line plane collision +/*! +*\return + -0 if the ray never intersects + -1 if the ray collides in front + -2 if the ray collides in back +*/ + +SIMD_FORCE_INLINE int bt_line_plane_collision( + 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)tmax) + { + returnvalue = 0; + tparam = tmax; + } + 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) +{ + btVector3 AD = vA2 - vA1; + btVector3 BD = vB2 - vB1; + btVector3 N = AD.cross(BD); + btScalar tp = N.length2(); + + btVector4 _M;//plane + + if(tp_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](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 ) + { + return ( a < b ); + } +}; + + +void btContactArray::merge_contacts( + const btContactArray & contacts, bool normal_contact_average) +{ + clear(); + + if(contacts.size()==0) return; + + + if(contacts.size()==1) + { + push_back(contacts[0]); + return; + } + + btAlignedObjectArray keycontacts; + + keycontacts.reserve(contacts.size()); + + //fill key contacts + + for (int i = 0;im_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) + { + *pcontact = *scontact; + coincident_count = 0; + } + else if(normal_contact_average) + { + if(btFabs(pcontact->m_depth - scontact->m_depth)m_normal; + coincident_count++; + } + } + } + } + else + {//add new contact + + 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]; + } + last_key = key; + } +} + +void btContactArray::merge_contacts_unique(const btContactArray & contacts) +{ + clear(); + + if(contacts.size()==0) return; + + if(contacts.size()==1) + { + push_back(contacts[0]); + return; + } + + BT_CONTACT average_contact = contacts[0]; + + for (int i=1;i splitValue) + { + //swap + primitive_boxes.swap(i,splitIndex); + //swapLeafNodes(i,splitIndex); + splitIndex++; + } + } + + //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex + //otherwise the tree-building might fail due to stack-overflows in certain cases. + //unbalanced1 is unsafe: it can cause stack overflows + //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); + + //unbalanced2 should work too: always use center (perfect balanced trees) + //bool unbalanced2 = true; + + //this should be safe too: + int rangeBalancedIndices = numIndices/3; + bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + + if (unbalanced) + { + splitIndex = startIndex+ (numIndices>>1); + } + + bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); + btAssert(!unbal); + + return splitIndex; + +} + + +void btBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +{ + int curIndex = m_num_nodes; + m_num_nodes++; + + btAssert((endIndex-startIndex)>0); + + if ((endIndex-startIndex)==1) + { + //We have a leaf node + setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); + + return; + } + //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); + + splitIndex = _sort_and_calc_splitting_index( + primitive_boxes,startIndex,endIndex, + splitIndex//split axis + ); + + + //calc this node bounding box + + btAABB node_bound; + node_bound.invalidate(); + + for (int i=startIndex;iget_primitive_box(getNodeData(nodecount),leafbox); + setNodeBound(nodecount,leafbox); + } + else + { + //const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); + //get left bound + btAABB bound; + bound.invalidate(); + + btAABB temp_box; + + int child_node = getLeftNode(nodecount); + if(child_node) + { + getNodeBound(child_node,temp_box); + bound.merge(temp_box); + } + + child_node = getRightNode(nodecount); + if(child_node) + { + getNodeBound(child_node,temp_box); + bound.merge(temp_box); + } + + setNodeBound(nodecount,bound); + } + } +} + +//! this rebuild the entire set +void btGImpactBvh::buildSet() +{ + //obtain primitive boxes + BT_BVH_DATA_ARRAY primitive_boxes; + primitive_boxes.resize(m_primitive_manager->get_primitive_count()); + + for (int i = 0;iget_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 & collided_results) const +{ + int curIndex = 0; + int numNodes = getNodeCount(); + + while (curIndex < numNodes) + { + btAABB bound; + getNodeBound(curIndex,bound); + + //catch bugs in tree data + + bool aabbOverlap = bound.has_collision(box); + bool isleafnode = isLeafNode(curIndex); + + if (isleafnode && aabbOverlap) + { + collided_results.push_back(getNodeData(curIndex)); + } + + if (aabbOverlap || isleafnode) + { + //next subnode + curIndex++; + } + else + { + //skip node + curIndex+= getEscapeNodeIndex(curIndex); + } + } + 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 & collided_results) const +{ + int curIndex = 0; + int numNodes = getNodeCount(); + + while (curIndex < numNodes) + { + btAABB bound; + getNodeBound(curIndex,bound); + + //catch bugs in tree data + + bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool isleafnode = isLeafNode(curIndex); + + if (isleafnode && aabbOverlap) + { + collided_results.push_back(getNodeData( curIndex)); + } + + if (aabbOverlap || isleafnode) + { + //next subnode + curIndex++; + } + else + { + //skip node + curIndex+= getEscapeNodeIndex(curIndex); + } + } + 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) +{ + btAABB 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); + +} + + +//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, + 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(boxset0->isLeafNode(node0)) + { + if(boxset1->isLeafNode(node1)) + { + // collision result + collision_pairs->push_pair( + 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); + + //collide right recursive + _find_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + node0,boxset1->getRightNode(node1),false); + + + } + } + else + { + if(boxset1->isLeafNode(node1)) + { + + //collide left recursive + _find_collision_pairs_recursive( + 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); + + + } + else + { + //collide left0 left1 + + + + _find_collision_pairs_recursive( + 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); + + + //collide right0 left1 + + _find_collision_pairs_recursive( + 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); + + }// 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) +{ + + if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; + + BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; + + trans_cache_1to0.calc_from_homogenic(trans0,trans1); + + bt_begin_gim02_tree_time(); + + _find_collision_pairs_recursive( + boxset0,boxset1, + &collision_pairs,trans_cache_1to0,0,0,true); + + bt_end_gim02_tree_time(); +} + diff --git a/Extras/GIMPACT/src/Bullet/btGImpactCollisionAlgorithm.cpp b/Extras/GIMPACT/src/Bullet/btGImpactCollisionAlgorithm.cpp index 313c7f82a..d1a3aead7 100755 --- a/Extras/GIMPACT/src/Bullet/btGImpactCollisionAlgorithm.cpp +++ b/Extras/GIMPACT/src/Bullet/btGImpactCollisionAlgorithm.cpp @@ -1,6 +1,11 @@ /* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. @@ -12,7 +17,6 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ - /* Author: Francisco León Nájera Concave-Concave Collision @@ -22,13 +26,10 @@ Concave-Concave Collision #include "BulletCollision/CollisionDispatch/btManifoldResult.h" #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "GIMPACT/Bullet/btGImpactCollisionAlgorithm.h" -#include "GIMPACT/core/gim_contact.h" #include "BulletCollision/CollisionShapes/btBoxShape.h" - -#define BULLET_TRIANGLE_COLLISION 1 -#define TREE_PRIMITIVE_VS_BOX true -#define GIMPACT_VS_PLANE_COLLISION 1 +#include "GIMPACT/Bullet/btGImpactCollisionAlgorithm.h" +#include "GIMPACT/Bullet/btContactProcessing.h" +#include "LinearMath/btQuickprof.h" //! Class for accessing the plane equation @@ -59,6 +60,134 @@ public: } }; + + +////////////////////////////////////////////////////////////////////////////////////////////// + +btClock g_triangle_clock; + +float g_accum_triangle_collision_time = 0; +int g_count_triangle_collision = 0; + +void bt_begin_gim02_tri_time() +{ + g_triangle_clock.reset(); +} + +void bt_end_gim02_tri_time() +{ + g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds(); + g_count_triangle_collision++; +} + +//! Retrieving shapes shapes +/*! +Declared here due of insuficent space on Pool allocators +*/ +//!@{ +class GIM_ShapeRetriever +{ +public: + btGImpactShapeInterface * m_gim_shape; + btTriangleShapeEx m_trishape; + btTetrahedronShapeEx m_tetrashape; + +public: + class ChildShapeRetriever + { + public: + GIM_ShapeRetriever * m_parent; + virtual btCollisionShape * getChildShape(int index) + { + return m_parent->m_gim_shape->getChildShape(index); + } + }; + + class TriangleShapeRetriever:public ChildShapeRetriever + { + public: + + virtual btCollisionShape * getChildShape(int index) + { + m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape); + return &m_parent->m_trishape; + } + }; + + class TetraShapeRetriever:public ChildShapeRetriever + { + public: + + virtual btCollisionShape * getChildShape(int index) + { + 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; + + GIM_ShapeRetriever(btGImpactShapeInterface * gim_shape) + { + m_gim_shape = gim_shape; + //select retriever + if(m_gim_shape->needsRetrieveTriangles()) + { + m_current_retriever = &m_tri_retriever; + } + else if(m_gim_shape->needsRetrieveTetrahedrons()) + { + m_current_retriever = &m_tetra_retriever; + } + else + { + m_current_retriever = &m_child_retriever; + } + + m_current_retriever->m_parent = this; + } + + btCollisionShape * getChildShape(int index) + { + return m_current_retriever->getChildShape(index); + } + + +}; + + + +//!@} + + + +//! 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; + + float avgtime = g_accum_triangle_collision_time; + avgtime /= (float)g_count_triangle_collision; + + g_accum_triangle_collision_time = 0; + g_count_triangle_collision = 0; + + return avgtime; +} + + + + btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) : btCollisionAlgorithm(ci) { @@ -73,50 +202,25 @@ btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm() -////////////////////////////////////////////////////////////////////////////////////////////// -void btGImpactCollisionAlgorithm::gimpactcompound_vs_gimpactcompound_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - btGImpactCompoundShape * shape0, - btGImpactCompoundShape * shape1,gim_pair_set & pairset) const +void btGImpactCollisionAlgorithm::addContactPoint(btCollisionObject * body0, + btCollisionObject * body1, + const btVector3 & point, + const btVector3 & normal, + btScalar distance) { - GIM_TREE_TREE_COLLIDER collider; - collider.find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset,TREE_PRIMITIVE_VS_BOX); - + m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1); + checkManifold(body0,body1); + m_resultOut->addContactPoint(normal,point,distance); } -void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_gimpacttrimeshpart_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - btGImpactMeshShapePart * shape0, - btGImpactMeshShapePart * shape1,gim_pair_set & pairset) const -{ - GIM_TREE_TREE_COLLIDER collider; - - collider.find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset,TREE_PRIMITIVE_VS_BOX); - -} - -void btGImpactCollisionAlgorithm::gimpactcompound_vs_gimpacttrimeshpart_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - btGImpactCompoundShape * shape0, - btGImpactMeshShapePart * shape1,gim_pair_set & pairset) const -{ - GIM_TREE_TREE_COLLIDER collider; - collider.find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset,TREE_PRIMITIVE_VS_BOX); -} - - - void btGImpactCollisionAlgorithm::shape_vs_shape_collision( btCollisionObject * body0, btCollisionObject * body1, btCollisionShape * shape0, - btCollisionShape * shape1,bool swapped) + btCollisionShape * shape1) { btCollisionShape * orgshape0 = body0->getCollisionShape(); @@ -125,28 +229,16 @@ void btGImpactCollisionAlgorithm::shape_vs_shape_collision( body0->setCollisionShape(shape0); body1->setCollisionShape(shape1); - - if(swapped) - { - btCollisionAlgorithm* algorswapped = newAlgorithm(body1,body0); - - m_resultOut->setPersistentManifold(m_manifoldPtr); - m_resultOut->setShapeIdentifiers(m_part1,m_triface1,m_part0,m_triface0); - - algorswapped->processCollision(body1,body0,*m_dispatchInfo,m_resultOut); - - delete algorswapped; - } - else { btCollisionAlgorithm* algor = newAlgorithm(body0,body1); + // post : checkManifold is called - m_resultOut->setPersistentManifold(m_manifoldPtr); m_resultOut->setShapeIdentifiers(m_part0,m_triface0,m_part1,m_triface1); algor->processCollision(body0,body1,*m_dispatchInfo,m_resultOut); - delete algor; + algor->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algor); } body0->setCollisionShape(orgshape0); @@ -159,8 +251,6 @@ void btGImpactCollisionAlgorithm::convex_vs_convex_collision( btCollisionShape * shape0, btCollisionShape * shape1) { - //shape_vs_shape_collision(body0,body1,shape0,shape1,false); - //return; btCollisionShape * orgshape0 = body0->getCollisionShape(); btCollisionShape * orgshape1 = body1->getCollisionShape(); @@ -181,543 +271,418 @@ void btGImpactCollisionAlgorithm::convex_vs_convex_collision( -void btGImpactCollisionAlgorithm::gimpacttrimesh_vs_shape_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShape * shape0, - btCollisionShape * shape1,bool swapped) + +void btGImpactCollisionAlgorithm::gimpact_vs_gimpact_find_pairs( + const btTransform & trans0, + const btTransform & trans1, + btGImpactShapeInterface * shape0, + btGImpactShapeInterface * shape1,btPairSet & pairset) { - GUINT i = shape0->getMeshPartCount(); - while(i--) + if(shape0->hasBoxSet() && shape1->hasBoxSet()) { - btGImpactMeshShapePart * part = shape0->getMeshPart(i); - gimpacttrimeshpart_vs_shape_collision(body0,body1,part,shape1,swapped); + btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset); } -} - -void btGImpactCollisionAlgorithm::gimpacttrimesh_vs_gimpacttrimesh( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShape * shape0, - btGImpactMeshShape * shape1) -{ - GUINT i = shape0->getMeshPartCount(); - while(i--) + else { - btGImpactMeshShapePart * part0 = shape0->getMeshPart(i); + btAABB boxshape0; + btAABB boxshape1; + int i = shape0->getNumChildShapes(); - GUINT j = shape1->getMeshPartCount(); - while(j--) + while(i--) { - btGImpactMeshShapePart * part1 = shape1->getMeshPart(j); - gimpacttrimeshpart_vs_gimpacttrimeshpart_collision(body0,body1,part0,part1,false); + shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + + int j = shape1->getNumChildShapes(); + while(j--) + { + shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max); + + if(boxshape1.has_collision(boxshape0)) + { + pairset.push_pair(i,j); + } + } } } + + } -void btGImpactCollisionAlgorithm::gimpacttrimesh_vs_gimpactcompound( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShape * shape0, - btGImpactCompoundShape * shape1,bool swapped) + +void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs( + const btTransform & trans0, + const btTransform & trans1, + btGImpactShapeInterface * shape0, + btCollisionShape * shape1, + btAlignedObjectArray & collided_primitives) { - GUINT i = shape0->getMeshPartCount(); - while(i--) + + btAABB boxshape; + + + if(shape0->hasBoxSet()) { - btGImpactMeshShapePart * part = shape0->getMeshPart(i); - gimpactcompound_vs_gimpacttrimeshpart_collision(body1,body0,shape1,part,!swapped); + btTransform trans1to0 = trans0.inverse(); + trans1to0 *= trans1; + + 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); + + btAABB boxshape0; + int i = shape0->getNumChildShapes(); + + while(i--) + { + shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); + + if(boxshape.has_collision(boxshape0)) + { + collided_primitives.push_back(i); + } + } + + } + } - -void btGImpactCollisionAlgorithm::gimpacttrimesh_vs_trimeshpart( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShape * shape0, - btGImpactMeshShapePart * shape1,bool swapped) +void btGImpactCollisionAlgorithm::collide_gjk_triangles(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactMeshShapePart * shape0, + btGImpactMeshShapePart * shape1, + const int * pairs, int pair_count) { - GUINT i = shape0->getMeshPartCount(); - while(i--) + btTriangleShapeEx tri0; + btTriangleShapeEx tri1; + + shape0->lockChildShapes(); + shape1->lockChildShapes(); + + const int * pair_pointer = pairs; + + while(pair_count--) { - btGImpactMeshShapePart * part = shape0->getMeshPart(i); - gimpacttrimeshpart_vs_gimpacttrimeshpart_collision(body0,body1,part,shape1,swapped); + + m_triface0 = *(pair_pointer); + 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)) + { + convex_vs_convex_collision(body0,body1,&tri0,&tri1); + } + } + + shape0->unlockChildShapes(); + shape1->unlockChildShapes(); } - -void btGImpactCollisionAlgorithm::gimpactcompound_vs_gimpactcompound_collision( - btCollisionObject * body0, +void btGImpactCollisionAlgorithm::collide_sat_triangles(btCollisionObject * body0, btCollisionObject * body1, - btGImpactCompoundShape * shape0, - btGImpactCompoundShape * shape1) + btGImpactMeshShapePart * shape0, + btGImpactMeshShapePart * shape1, + const int * pairs, int pair_count) { btTransform orgtrans0 = body0->getWorldTransform(); btTransform orgtrans1 = body1->getWorldTransform(); - gim_pair_set pairset; + btPrimitiveTriangle ptri0; + btPrimitiveTriangle ptri1; + BT_TRIANGLE_CONTACT contact_data; - gimpactcompound_vs_gimpactcompound_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset); + shape0->lockChildShapes(); + shape1->lockChildShapes(); - if(pairset.size()== 0) return; + const int * pair_pointer = pairs; -// btCollisionShape * orgshape0 = body0->getCollisionShape(); -// btCollisionShape * orgshape1 = body1->getCollisionShape(); - - GUINT i = pairset.size(); - while(i--) + while(pair_count--) { - const GIM_PAIR & pair = pairset[i]; - btCollisionShape * colshape0 = shape0->getChildShape(pair.m_index1); - btCollisionShape * colshape1 = shape1->getChildShape(pair.m_index2); - btTransform childtrans0 = orgtrans0*shape0->getChildTransform(pair.m_index1); - btTransform childtrans1 = orgtrans1*shape1->getChildTransform(pair.m_index2); - - body0->setWorldTransform(childtrans0); - body1->setWorldTransform(childtrans1); + m_triface0 = *(pair_pointer); + m_triface1 = *(pair_pointer+1); + pair_pointer+=2; - //collide two shapes - shape_vs_shape_collision(body0,body1,colshape0,colshape1,false); + shape0->getPrimitiveTriangle(m_triface0,ptri0); + shape1->getPrimitiveTriangle(m_triface1,ptri1); - //restore transforms -// body0->setCollisionShape(orgshape0); -// body1->setCollisionShape(orgshape1); + #ifdef TRI_COLLISION_PROFILING + bt_begin_gim02_tri_time(); + #endif + + ptri0.applyTransform(orgtrans0); + ptri1.applyTransform(orgtrans1); + + + //build planes + ptri0.buildTriPlane(); + ptri1.buildTriPlane(); + // test conservative + + + + if(ptri0.overlap_test_conservative(ptri1)) + { + if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data)) + { + + int j = contact_data.m_point_count; + while(j--) + { + + addContactPoint(body0, body1, + contact_data.m_points[j], + contact_data.m_separating_normal, + -contact_data.m_penetration_depth); + } + } + } + + #ifdef TRI_COLLISION_PROFILING + bt_end_gim02_tri_time(); + #endif - body0->setWorldTransform(orgtrans0); - body1->setWorldTransform(orgtrans1); } + + shape0->unlockChildShapes(); + shape1->unlockChildShapes(); + } -void btGImpactCollisionAlgorithm::gimpactcompound_vs_gimpacttrimeshpart_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactCompoundShape * shape0, - btGImpactMeshShapePart * shape1,bool swapped) +void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( + btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btGImpactShapeInterface * shape1) { - //lock trimesh - shape1->lock(); - btTransform orgtrans0 = body0->getWorldTransform(); - btTransform orgtrans1 = body1->getWorldTransform(); - - gim_pair_set pairset; - - gimpactcompound_vs_gimpacttrimeshpart_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset); - - if(pairset.size()== 0) + if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) { - //unlock trimesh - shape1->unlock(); + btGImpactMeshShape * meshshape0 = static_cast(shape0); + m_part0 = meshshape0->getMeshPartCount(); + + while(m_part0--) + { + gimpact_vs_gimpact(body0,body1,meshshape0->getMeshPart(m_part0),shape1); + } + return; } -// btCollisionShape * orgshape0 = body0->getCollisionShape(); -// btCollisionShape * orgshape1 = body1->getCollisionShape(); - - - m_part1 = shape1->getPart(); - m_part0 = -1; - m_triface0 = -1; - - btTriangleShapeEx bullet_triangle(btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f)); - - - GUINT i = pairset.size(); - - while(i--) + if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) { - const GIM_PAIR & pair = pairset[i]; - btCollisionShape * colshape0 = shape0->getChildShape(pair.m_index1); - btTransform childtrans0 = orgtrans0*shape0->getChildTransform(pair.m_index1); - body0->setWorldTransform(childtrans0); + btGImpactMeshShape * meshshape1 = static_cast(shape1); + m_part1 = meshshape1->getMeshPartCount(); - shape1->getBulletTriangle(pair.m_index2,bullet_triangle); - m_triface1 = pair.m_index2; + while(m_part1--) + { - //collide two shapes - shape_vs_shape_collision(body0,body1,colshape0,&bullet_triangle,swapped); + gimpact_vs_gimpact(body0,body1,shape0,meshshape1->getMeshPart(m_part1)); - //restore transforms -// body0->setCollisionShape(orgshape0); -// body1->setCollisionShape(orgshape1); - body1->setWorldTransform(orgtrans1); + } + + return; } - //unlock trimesh - shape1->unlock(); + + btTransform orgtrans0 = body0->getWorldTransform(); + btTransform orgtrans1 = body1->getWorldTransform(); + + btPairSet pairset; + + gimpact_vs_gimpact_find_pairs(orgtrans0,orgtrans1,shape0,shape1,pairset); + + if(pairset.size()== 0) return; + + if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && + shape1->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART) + { + btGImpactMeshShapePart * shapepart0 = static_cast(shape0); + btGImpactMeshShapePart * shapepart1 = static_cast(shape1); + //specialized function + #ifdef BULLET_TRIANGLE_COLLISION + collide_gjk_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); + #else + collide_sat_triangles(body0,body1,shapepart0,shapepart1,&pairset[0].m_index1,pairset.size()); + #endif + + return; + } + + //general function + + shape0->lockChildShapes(); + shape1->lockChildShapes(); + + GIM_ShapeRetriever retriever0(shape0); + GIM_ShapeRetriever retriever1(shape1); + + bool child_has_transform0 = shape0->childrenHasTransform(); + bool child_has_transform1 = shape1->childrenHasTransform(); + + int i = pairset.size(); + while(i--) + { + BT_PAIR * pair = &pairset[i]; + m_triface0 = pair->m_index1; + m_triface1 = pair->m_index2; + btCollisionShape * colshape0 = retriever0.getChildShape(m_triface0); + btCollisionShape * colshape1 = retriever1.getChildShape(m_triface1); + + if(child_has_transform0) + { + body0->setWorldTransform(orgtrans0*shape0->getChildTransform(m_triface0)); + } + + if(child_has_transform1) + { + body1->setWorldTransform(orgtrans1*shape1->getChildTransform(m_triface1)); + } + + //collide two convex shapes + convex_vs_convex_collision(body0,body1,colshape0,colshape1); + + + if(child_has_transform0) + { + body0->setWorldTransform(orgtrans0); + } + + if(child_has_transform1) + { + body1->setWorldTransform(orgtrans1); + } + + } + + shape0->unlockChildShapes(); + shape1->unlockChildShapes(); } - - -void btGImpactCollisionAlgorithm::gimpactcompound_vs_shape_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactCompoundShape * shape0, - btCollisionShape * shape1,bool swapped) +void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btCollisionShape * shape1,bool swapped) { + if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) + { + btGImpactMeshShape * meshshape0 = static_cast(shape0); + m_part0 = meshshape0->getMeshPartCount(); + + while(m_part0--) + { + + gimpact_vs_shape(body0, + body1, + meshshape0->getMeshPart(m_part0), + shape1,swapped); + + } + + return; + } + + #ifdef GIMPACT_VS_PLANE_COLLISION + if(shape0->getGImpactShapeType() == CONST_GIMPACT_TRIMESH_SHAPE_PART && + shape1->getShapeType() == STATIC_PLANE_PROXYTYPE) + { + btGImpactMeshShapePart * shapepart = static_cast(shape0); + btStaticPlaneShape * planeshape = static_cast(shape1); + gimpacttrimeshpart_vs_plane_collision(body0,body1,shapepart,planeshape,swapped); + return; + } + + #endif + + + + if(shape1->isCompound()) + { + btCompoundShape * compoundshape = static_cast(shape1); + gimpact_vs_compoundshape(body0,body1,shape0,compoundshape,swapped); + return; + } + else if(shape1->isConcave()) + { + btConcaveShape * concaveshape = static_cast(shape1); + gimpact_vs_concave(body0,body1,shape0,concaveshape,swapped); + return; + } + + btTransform orgtrans0 = body0->getWorldTransform(); - btTransform trans1to0 = orgtrans0.inverse(); - trans1to0 *= body1->getWorldTransform(); + btTransform orgtrans1 = body1->getWorldTransform(); - GIM_AABB boxshape; - shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max); - gim_array collided_results; - shape0->getBoxSet()->boxQuery(boxshape, collided_results); + btAlignedObjectArray collided_results; + + gimpact_vs_shape_find_pairs(orgtrans0,orgtrans1,shape0,shape1,collided_results); if(collided_results.size() == 0) return; + shape0->lockChildShapes(); - GUINT i = collided_results.size(); + GIM_ShapeRetriever retriever0(shape0); + + + bool child_has_transform0 = shape0->childrenHasTransform(); + + + int i = collided_results.size(); while(i--) { - btCollisionShape * colshape0 = shape0->getChildShape(collided_results[i]); - btTransform childtrans0 = orgtrans0*shape0->getChildTransform(collided_results[i]); + int child_index = collided_results[i]; + m_triface0 = child_index; - body0->setWorldTransform(childtrans0); + btCollisionShape * colshape0 = retriever0.getChildShape(child_index); + + if(child_has_transform0) + { + body0->setWorldTransform(orgtrans0*shape0->getChildTransform(child_index)); + } //collide two shapes - shape_vs_shape_collision(body0,body1,colshape0,shape1,swapped); - - //restore transforms -// body0->setCollisionShape(orgshape0); - body0->setWorldTransform(orgtrans0); - } - -} - - - - -void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_gimpacttrimeshpart_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShapePart * shape0, - btGImpactMeshShapePart * shape1,bool swapped) -{ - shape0->lock(); - shape1->lock(); - - btGImpactMeshShapePart * trishape0; - btGImpactMeshShapePart * trishape1; - btCollisionObject * tribody0; - btCollisionObject * tribody1; - - if(swapped) - { - trishape0 = shape1; - trishape1 = shape0; - tribody0 = body1; - tribody1 = body0; - } - else - { - trishape0 = shape0; - trishape1 = shape1; - tribody0 = body0; - tribody1 = body1; - } - - btTransform orgtrans0 = tribody0->getWorldTransform(); - btTransform orgtrans1 = tribody1->getWorldTransform(); - - gim_pair_set pairset; - - gimpacttrimeshpart_vs_gimpacttrimeshpart_find_pairs(orgtrans0,orgtrans1,trishape0,trishape1,pairset); - - if(pairset.size()== 0) - { - shape0->unlock(); - shape1->unlock(); - return; - } - - m_part0 = trishape0->getPart(); - m_part1 = trishape1->getPart(); - -#ifdef BULLET_TRIANGLE_COLLISION - - btTriangleShapeEx tri0(btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f)); - btTriangleShapeEx tri1(btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f)); - GUINT i = pairset.size(); - while(i--) - { - const GIM_PAIR & pair = pairset[i]; - m_triface0 = pair.m_index1; - m_triface1 = pair.m_index2; - - trishape0->getBulletTriangle(pair.m_index1,tri0); - trishape1->getBulletTriangle(pair.m_index2,tri1); - - - convex_vs_convex_collision( - tribody0, - tribody1, - &tri0, - &tri1); - - } - //unlock - shape0->unlock(); - shape1->unlock(); - -#else - gim_contact_array tempcontacts; - GIM_TRIANGLE tri0; - GIM_TRIANGLE tri1; - GIM_TRIANGLE_CONTACT_DATA contact_data; - GUINT i = pairset.size(); - while(i--) - { - const GIM_PAIR & pair = pairset[i]; - - trishape0->getTriangle(pair.m_index1,tri0); - trishape1->getTriangle(pair.m_index2,tri1); - - tri0.apply_transform(orgtrans0); - tri1.apply_transform(orgtrans1); - - if(tri0.collide_triangle(tri1,contact_data)) - { - tempcontacts.push_triangle_contacts(contact_data,pair.m_index1,pair.m_index2); - } - } - //unlock - shape0->unlock(); - shape1->unlock(); - - if(tempcontacts.size()==0) return; - - //sort contacts - gim_contact_array contacts; - contacts.merge_contacts(tempcontacts,true); - // put contacts - m_part0 = trishape0->getPart(); - m_part1 = trishape1->getPart(); - i = contacts.size(); - while(i--) - { - GIM_CONTACT * pcontact = &contacts[i]; - - m_triface0 = pcontact->m_feature1; - m_triface1 = pcontact->m_feature2; - - addContactPoint(tribody0, tribody1, - pcontact->m_point, - pcontact->m_normal, - -pcontact->m_depth); - } -#endif -} - -void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShapePart * shape0, - btStaticPlaneShape * shape1,bool swapped) -{ - - - btTransform orgtrans0 = body0->getWorldTransform(); - btTransform orgtrans1 = body1->getWorldTransform(); - - btPlaneShape * planeshape = static_cast(shape1); - btVector4 plane; - planeshape->get_plane_equation_transformed(orgtrans1,plane); - - //test box against plane - - GIM_AABB tribox; - shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max); - tribox.increment_margin(planeshape->getMargin()); - - if( tribox.plane_classify(plane)!= G_COLLIDE_PLANE) return; - - shape0->lock(); - - GREAL margin = shape0->getMargin() + planeshape->getMargin(); - - btVector3 vertex; - GUINT vi = shape0->getVertexCount(); - while(vi--) - { - shape0->getVertex(vi,vertex); - vertex = orgtrans0(vertex); - - GREAL distance = vertex.dot(plane) - plane[3] - margin; - - if(distance<0.0)//add contact - { - if(swapped) - { - addContactPoint(body1, body0, - vertex, - -plane, - distance); - } - else - { - addContactPoint(body0, body1, - vertex, - plane, - distance); - } - } - } - - shape0->unlock(); -} - - -class btGImpactTriangleCallback: public btTriangleCallback -{ -public: - btGImpactCollisionAlgorithm * algorithm; - btCollisionObject * body0; - btCollisionObject * body1; - btGImpactMeshShapePart * gimpactshape0; - bool swapped; - btScalar margin; - - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) - { - btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]); - tri1.setMargin(margin); - algorithm->gimpacttrimeshpart_vs_shape_collision( - body0,body1,gimpactshape0,&tri1,swapped); - } -}; - - - -void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_concave_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShapePart * shape0, - btConcaveShape * shape1,bool swapped) -{ - - //create the callback - btGImpactTriangleCallback tricallback; - tricallback.algorithm = this; - tricallback.body0 = body0; - tricallback.body1 = body1; - tricallback.gimpactshape0 = shape0; - tricallback.swapped = swapped; - tricallback.margin = shape1->getMargin(); - - - - //getting the trimesh AABB - btTransform gimpactInConcaveSpace; - gimpactInConcaveSpace = body1->getWorldTransform().inverse() * body0->getWorldTransform(); - btVector3 minAABB,maxAABB; - shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB); - - shape1->processAllTriangles(&tricallback,minAABB,maxAABB); - - - -} - - -void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_shape_collision( - btCollisionObject * body0, - btCollisionObject * body1, - btGImpactMeshShapePart * shape0, - btCollisionShape * shape1,bool swapped) -{ -#ifdef GIMPACT_VS_PLANE_COLLISION - if(shape1->getShapeType() == STATIC_PLANE_PROXYTYPE) - { - btStaticPlaneShape * plane1 = static_cast(shape1); - gimpacttrimeshpart_vs_plane_collision(body0,body1,shape0,plane1,swapped); - return; - } -#endif - if(shape1->isConcave()) - { - btConcaveShape * concave1 = static_cast(shape1); - gimpacttrimeshpart_vs_concave_collision(body0,body1,shape0,concave1,swapped); - return; - } - - btTransform trans1to0 = body0->getWorldTransform().inverse(); - trans1to0 *= body1->getWorldTransform(); - - //lock - shape0->lock(); - - GIM_AABB boxshape; - shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max); - gim_array collided_results(32); - shape0->getBoxSet()->boxQuery(boxshape, collided_results); - - if(collided_results.size() == 0) - { - shape0->unlock(); - return; - } - - btTriangleShapeEx bullet_triangle(btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f),btVector3(0.f,0.f,0.f)); - - - m_part0 = shape0->getPart(); - m_part1 = -1; - m_triface1 = -1; - - GUINT i = collided_results.size(); - - if(shape1->isConvex()) - { if(swapped) { - while(i--) - { - m_triface0 = collided_results[i]; - shape0->getBulletTriangle(collided_results[i],bullet_triangle); - //collide two shapes - convex_vs_convex_collision(body1,body0,shape1,&bullet_triangle); - } + shape_vs_shape_collision(body1,body0,shape1,colshape0); } else { - while(i--) - { - m_triface0 = collided_results[i]; - shape0->getBulletTriangle(collided_results[i],bullet_triangle); - //collide two shapes - convex_vs_convex_collision(body0,body1,&bullet_triangle,shape1); - } + shape_vs_shape_collision(body0,body1,colshape0,shape1); } - } - else - { - while(i--) + + //restore transforms + if(child_has_transform0) { - m_triface0 = collided_results[i]; - shape0->getBulletTriangle(collided_results[i],bullet_triangle); - //collide two shapes - shape_vs_shape_collision(body0,body1,&bullet_triangle,shape1,swapped); + body0->setWorldTransform(orgtrans0); } + } + shape0->unlockChildShapes(); - - shape0->unlock(); } - void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(btCollisionObject * body0, - btCollisionObject * body1, - btGImpactShapeInterface * shape0, - btCompoundShape * shape1,bool swapped) + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btCompoundShape * shape1,bool swapped) { btTransform orgtrans1 = body1->getWorldTransform(); @@ -740,153 +705,147 @@ void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(btCollisionObject * b } } - -void btGImpactCollisionAlgorithm::gimpact_vs_shape(btCollisionObject * body0, +void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( + btCollisionObject * body0, btCollisionObject * body1, - btGImpactShapeInterface * shape0, - btCollisionShape * shape1,bool swapped) -{ - if(shape1->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) - { - btCompoundShape * compoundshape = static_cast(shape1); - gimpact_vs_compoundshape(body0,body1,shape0,compoundshape,swapped); - return; - } - - eGIMPACT_SHAPE_TYPE shapetype0 = shape0->getGImpactShapeType(); - if(shapetype0 == CONST_GIMPACT_TRIMESH_SHAPE) - { - btGImpactMeshShape * trimesh0 = static_cast(shape0); - gimpacttrimesh_vs_shape_collision(body0,body1,trimesh0,shape1,swapped); - } - else if(shapetype0 == CONST_GIMPACT_TRIMESH_SHAPE_PART) - { - btGImpactMeshShapePart * trimeshpart0 = static_cast(shape0); - gimpacttrimeshpart_vs_shape_collision(body0,body1,trimeshpart0,shape1,swapped); - } - else// compound - { - btGImpactCompoundShape * compound0 = static_cast(shape0); - gimpactcompound_vs_shape_collision(body0,body1,compound0,shape1,swapped); - } -} - -void btGImpactCollisionAlgorithm::gimpact_vs_gimpact(btCollisionObject * body0, - btCollisionObject * body1, - btGImpactShapeInterface * shape0, - btGImpactShapeInterface * shape1) + btGImpactMeshShapePart * shape0, + btStaticPlaneShape * shape1,bool swapped) { - eGIMPACT_SHAPE_TYPE shapetype0 = shape0->getGImpactShapeType(); - eGIMPACT_SHAPE_TYPE shapetype1 = shape1->getGImpactShapeType(); - btGImpactMeshShape * trimesh0; - btGImpactMeshShape * trimesh1; - btGImpactMeshShapePart * trimeshpart0; - btGImpactMeshShapePart * trimeshpart1; - btGImpactCompoundShape * compound0; - btGImpactCompoundShape * compound1; + btTransform orgtrans0 = body0->getWorldTransform(); + btTransform orgtrans1 = body1->getWorldTransform(); + btPlaneShape * planeshape = static_cast(shape1); + btVector4 plane; + planeshape->get_plane_equation_transformed(orgtrans1,plane); - if(shapetype0 == CONST_GIMPACT_TRIMESH_SHAPE) + //test box against plane + + btAABB tribox; + shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max); + tribox.increment_margin(planeshape->getMargin()); + + if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return; + + shape0->lockChildShapes(); + + btScalar margin = shape0->getMargin() + planeshape->getMargin(); + + btVector3 vertex; + int vi = shape0->getVertexCount(); + while(vi--) { - if(shapetype1 == CONST_GIMPACT_TRIMESH_SHAPE) // trimesh vs trimesh + shape0->getVertex(vi,vertex); + vertex = orgtrans0(vertex); + + btScalar distance = vertex.dot(plane) - plane[3] - margin; + + if(distance<0.0)//add contact { - trimesh0 = static_cast(shape0); - trimesh1 = static_cast(shape1); - gimpacttrimesh_vs_gimpacttrimesh(body0,body1,trimesh0,trimesh1); - } - else - { - if(shapetype1 == CONST_GIMPACT_COMPOUND_SHAPE) // trimesh vs compound + if(swapped) { - trimesh0 = static_cast(shape0); - compound1 = static_cast(shape1); - gimpacttrimesh_vs_gimpactcompound(body0,body1,trimesh0,compound1,false); + addContactPoint(body1, body0, + vertex, + -plane, + distance); } - else // trimesh vs trimesh part + else { - trimesh0 = static_cast(shape0); - trimeshpart1 = static_cast(shape1); - gimpacttrimesh_vs_trimeshpart(body0,body1,trimesh0,trimeshpart1,false); + addContactPoint(body0, body1, + vertex, + plane, + distance); } } } - else if(shapetype1 == CONST_GIMPACT_TRIMESH_SHAPE) - { - if(shapetype0 == CONST_GIMPACT_COMPOUND_SHAPE) // compound vs trimesh - { - compound0 = static_cast(shape0); - trimesh1 = static_cast(shape1); - gimpacttrimesh_vs_gimpactcompound(body1,body0,trimesh1,compound0,true); - } - else // trimesh part vs trimesh - { - trimeshpart0 = static_cast(shape0); - trimesh1 = static_cast(shape1); - gimpacttrimesh_vs_trimeshpart(body1,body0,trimesh1,trimeshpart0,true); - } - } - else - { - if(shapetype0 == CONST_GIMPACT_COMPOUND_SHAPE) - { - if(shapetype1 == CONST_GIMPACT_COMPOUND_SHAPE) // compound vs compound - { - compound0 = static_cast(shape0); - compound1 = static_cast(shape1); - gimpactcompound_vs_gimpactcompound_collision(body0,body1,compound0,compound1); - } - else // compound vs trimesh part - { - compound0 = static_cast(shape0); - trimeshpart1 = static_cast(shape1); - gimpactcompound_vs_gimpacttrimeshpart_collision(body0,body1,compound0,trimeshpart1,false); - } - } - else if(shapetype1 == CONST_GIMPACT_COMPOUND_SHAPE) // trimesh part vs compound - { - compound1 = static_cast(shape1); - trimeshpart0 = static_cast(shape0); - gimpactcompound_vs_gimpacttrimeshpart_collision(body1,body0,compound1,trimeshpart0,true); - } - else // trimesh part vs trimesh part - { - trimeshpart0 = static_cast(shape0); - trimeshpart1 = static_cast(shape1); - gimpacttrimeshpart_vs_gimpacttrimeshpart_collision(body0,body1,trimeshpart0,trimeshpart1,false); - } - } + shape0->unlockChildShapes(); } + + + +class btGImpactTriangleCallback: public btTriangleCallback +{ +public: + btGImpactCollisionAlgorithm * algorithm; + btCollisionObject * body0; + btCollisionObject * body1; + btGImpactShapeInterface * gimpactshape0; + bool swapped; + btScalar margin; + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + btTriangleShapeEx tri1(triangle[0],triangle[1],triangle[2]); + tri1.setMargin(margin); + algorithm->gimpact_vs_shape( + body0,body1,gimpactshape0,&tri1,swapped); + } +}; + + + + +void btGImpactCollisionAlgorithm::gimpact_vs_concave( + btCollisionObject * body0, + btCollisionObject * body1, + btGImpactShapeInterface * shape0, + btConcaveShape * shape1,bool swapped) +{ + //create the callback + btGImpactTriangleCallback tricallback; + tricallback.algorithm = this; + tricallback.body0 = body0; + tricallback.body1 = body1; + tricallback.gimpactshape0 = shape0; + tricallback.swapped = swapped; + tricallback.margin = shape1->getMargin(); + + //getting the trimesh AABB + btTransform gimpactInConcaveSpace; + + gimpactInConcaveSpace = body1->getWorldTransform().inverse() * body0->getWorldTransform(); + + btVector3 minAABB,maxAABB; + shape0->getAabb(gimpactInConcaveSpace,minAABB,maxAABB); + + shape1->processAllTriangles(&tricallback,minAABB,maxAABB); + +} + + + void btGImpactCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { clearCache(); m_resultOut = resultOut; m_dispatchInfo = &dispatchInfo; - - btGImpactShapeInterface * gimpactshape0; btGImpactShapeInterface * gimpactshape1; + if (body0->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE) { + gimpactshape0 = static_cast(body0->getCollisionShape()); + if( body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) { - gimpactshape0 = static_cast(body0->getCollisionShape()); gimpactshape1 = static_cast(body1->getCollisionShape()); + gimpact_vs_gimpact(body0,body1,gimpactshape0,gimpactshape1); } else { - gimpactshape0 = static_cast(body0->getCollisionShape()); gimpact_vs_shape(body0,body1,gimpactshape0,body1->getCollisionShape(),false); } + } else if (body1->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) { gimpactshape1 = static_cast(body1->getCollisionShape()); + gimpact_vs_shape(body1,body0,gimpactshape1,body0->getCollisionShape(),true); } } @@ -900,25 +859,20 @@ btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* b ///////////////////////////////////// REGISTERING ALGORITHM ////////////////////////////////////////////// +btGImpactCollisionAlgorithm::CreateFunc g_gimpact_cf; //! Use this function for register the algorithm externally void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) { - for (GUINT i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (int i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) { - dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,new btGImpactCollisionAlgorithm::CreateFunc); + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&g_gimpact_cf); } - for (GUINT i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (int i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) { - dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,new btGImpactCollisionAlgorithm::CreateFunc); + dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&g_gimpact_cf); } - /* - dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,GIMPACT_SHAPE_PROXYTYPE,new btGImpactCollisionAlgorithm::CreateFunc); - dispatcher->registerCollisionCreateFunc(STATIC_PLANE_PROXYTYPE ,GIMPACT_SHAPE_PROXYTYPE,new btGImpactCollisionAlgorithm::CreateFunc); - dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,STATIC_PLANE_PROXYTYPE ,new btGImpactCollisionAlgorithm::CreateFunc);*/ - - } diff --git a/Extras/GIMPACT/src/Bullet/btGImpactQuantizedBvh.cpp b/Extras/GIMPACT/src/Bullet/btGImpactQuantizedBvh.cpp new file mode 100644 index 000000000..06754c35c --- /dev/null +++ b/Extras/GIMPACT/src/Bullet/btGImpactQuantizedBvh.cpp @@ -0,0 +1,523 @@ +/*! \file gim_box_set.h +\author Francisco León Nájera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "GIMPACT/Bullet/btGImpactQuantizedBvh.h" +#include "LinearMath/btQuickprof.h" + +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(); +} + +void bt_end_gim02_q_tree_time() +{ + g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds(); + g_q_count_traversing++; +} + + +//! Gets the average time in miliseconds of tree collisions +float btGImpactQuantizedBvh::getAverageTreeCollisionTime() +{ + if(g_q_count_traversing == 0) return 0; + + float avgtime = g_q_accum_tree_collision_time; + avgtime /= (float)g_q_count_traversing; + + g_q_accum_tree_collision_time = 0; + g_q_count_traversing = 0; + return avgtime; + +// float avgtime = g_q_count_traversing; +// g_q_count_traversing = 0; +// return avgtime; + +} + + +/////////////////////// btQuantizedBvhTree ///////////////////////////////// + +void btQuantizedBvhTree::calc_quantization( + BT_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) +{ + //calc globa box + btAABB global_bound; + global_bound.invalidate(); + + for (int i=0;i splitValue) + { + //swap + primitive_boxes.swap(i,splitIndex); + //swapLeafNodes(i,splitIndex); + splitIndex++; + } + } + + //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex + //otherwise the tree-building might fail due to stack-overflows in certain cases. + //unbalanced1 is unsafe: it can cause stack overflows + //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); + + //unbalanced2 should work too: always use center (perfect balanced trees) + //bool unbalanced2 = true; + + //this should be safe too: + int rangeBalancedIndices = numIndices/3; + bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + + if (unbalanced) + { + splitIndex = startIndex+ (numIndices>>1); + } + + bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); + btAssert(!unbal); + + return splitIndex; + +} + + +void btQuantizedBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) +{ + int curIndex = m_num_nodes; + m_num_nodes++; + + btAssert((endIndex-startIndex)>0); + + if ((endIndex-startIndex)==1) + { + //We have a leaf node + setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); + m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); + + return; + } + //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); + + splitIndex = _sort_and_calc_splitting_index( + primitive_boxes,startIndex,endIndex, + splitIndex//split axis + ); + + + //calc this node bounding box + + btAABB node_bound; + node_bound.invalidate(); + + for (int i=startIndex;iget_primitive_box(getNodeData(nodecount),leafbox); + setNodeBound(nodecount,leafbox); + } + else + { + //const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); + //get left bound + btAABB bound; + bound.invalidate(); + + btAABB temp_box; + + int child_node = getLeftNode(nodecount); + if(child_node) + { + getNodeBound(child_node,temp_box); + bound.merge(temp_box); + } + + child_node = getRightNode(nodecount); + if(child_node) + { + getNodeBound(child_node,temp_box); + bound.merge(temp_box); + } + + setNodeBound(nodecount,bound); + } + } +} + +//! this rebuild the entire set +void btGImpactQuantizedBvh::buildSet() +{ + //obtain primitive boxes + BT_BVH_DATA_ARRAY primitive_boxes; + primitive_boxes.resize(m_primitive_manager->get_primitive_count()); + + for (int i = 0;iget_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 & collided_results) const +{ + int curIndex = 0; + int numNodes = getNodeCount(); + + //quantize box + + unsigned short quantizedMin[3]; + unsigned short quantizedMax[3]; + + 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 isleafnode = isLeafNode(curIndex); + + if (isleafnode && aabbOverlap) + { + collided_results.push_back(getNodeData(curIndex)); + } + + if (aabbOverlap || isleafnode) + { + //next subnode + curIndex++; + } + else + { + //skip node + curIndex+= getEscapeNodeIndex(curIndex); + } + } + 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 & collided_results) const +{ + int curIndex = 0; + int numNodes = getNodeCount(); + + while (curIndex < numNodes) + { + btAABB bound; + getNodeBound(curIndex,bound); + + //catch bugs in tree data + + bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); + bool isleafnode = isLeafNode(curIndex); + + if (isleafnode && aabbOverlap) + { + collided_results.push_back(getNodeData( curIndex)); + } + + if (aabbOverlap || isleafnode) + { + //next subnode + curIndex++; + } + else + { + //skip node + curIndex+= getEscapeNodeIndex(curIndex); + } + } + if(collided_results.size()>0) return true; + return false; +} + + +SIMD_FORCE_INLINE bool _quantized_node_collision( + btGImpactQuantizedBvh * boxset0, 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); + 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); + +} + + +//stackless recursive collision routine +static void _find_quantized_collision_pairs_recursive( + btGImpactQuantizedBvh * boxset0, 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(boxset0->isLeafNode(node0)) + { + if(boxset1->isLeafNode(node1)) + { + // collision result + collision_pairs->push_pair( + 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); + + //collide right recursive + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + collision_pairs,trans_cache_1to0, + node0,boxset1->getRightNode(node1),false); + + + } + } + else + { + if(boxset1->isLeafNode(node1)) + { + + //collide left recursive + _find_quantized_collision_pairs_recursive( + 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); + + + } + else + { + //collide left0 left1 + + + + _find_quantized_collision_pairs_recursive( + 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); + + + //collide right0 left1 + + _find_quantized_collision_pairs_recursive( + 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); + + }// else if node1 is not a leaf + }// else if node0 is not a leaf +} + + +void btGImpactQuantizedBvh::find_collision(btGImpactQuantizedBvh * boxset0, const btTransform & trans0, + btGImpactQuantizedBvh * boxset1, const btTransform & trans1, + btPairSet & collision_pairs) +{ + + if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; + + BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; + + trans_cache_1to0.calc_from_homogenic(trans0,trans1); + + bt_begin_gim02_q_tree_time(); + + _find_quantized_collision_pairs_recursive( + boxset0,boxset1, + &collision_pairs,trans_cache_1to0,0,0,true); + + bt_end_gim02_q_tree_time(); +} + + diff --git a/Extras/GIMPACT/src/Bullet/btGImpactShape.cpp b/Extras/GIMPACT/src/Bullet/btGImpactShape.cpp index 18fccce24..7ebfd509d 100755 --- a/Extras/GIMPACT/src/Bullet/btGImpactShape.cpp +++ b/Extras/GIMPACT/src/Bullet/btGImpactShape.cpp @@ -1,30 +1,21 @@ /* ------------------------------------------------------------------------------ This source file is part of GIMPACT Library. For the latest info, see http://gimpact.sourceforge.net/ -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. email: projectileman@yahoo.com - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: ------------------------------------------------------------------------------ +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. */ @@ -36,18 +27,26 @@ email: projectileman@yahoo.com void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) { - + lockChildShapes(); #ifdef CALC_EXACT_INERTIA inertia.setValue(0.f,0.f,0.f); - GUINT i = this->getNumChildShapes(); - GREAL shapemass = mass/btScalar(i); + int i = this->getNumChildShapes(); + btScalar shapemass = mass/btScalar(i); while(i--) { btVector3 temp_inertia; m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia); - inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]); + if(childrenHasTransform()) + { + inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]); + } + else + { + inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity()); + } + } #else @@ -65,20 +64,21 @@ void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& iner inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); #endif + unlockChildShapes(); } void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) { - lock(); + lockChildShapes(); #ifdef CALC_EXACT_INERTIA inertia.setValue(0.f,0.f,0.f); - GUINT i = this->getVertexCount(); - GREAL pointmass = mass/btScalar(i); + int i = this->getVertexCount(); + btScalar pointmass = mass/btScalar(i); while(i--) { @@ -104,7 +104,7 @@ void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& iner #endif - unlock(); + unlockChildShapes(); } void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) @@ -113,8 +113,8 @@ void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) #ifdef CALC_EXACT_INERTIA inertia.setValue(0.f,0.f,0.f); - GUINT i = this->getMeshPartCount(); - GREAL partmass = mass/btScalar(i); + int i = this->getMeshPartCount(); + btScalar partmass = mass/btScalar(i); while(i--) { @@ -140,53 +140,42 @@ void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) #endif } - - -void btGImpactCompoundShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const -{ - -} - void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const { } -void btGImpactMeshShapePart::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const -{ -} - void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { - lock(); - GIM_AABB box; + lockChildShapes(); + btAABB box; box.m_min = aabbMin; box.m_max = aabbMax; - gim_array collided; + btAlignedObjectArray collided; m_box_set.boxQuery(box,collided); if(collided.size()==0) { - unlock(); + unlockChildShapes(); return; } int part = (int)getPart(); - GIM_TRIANGLE triangle; - GUINT i = collided.size(); + btPrimitiveTriangle triangle; + int i = collided.size(); while(i--) { - this->getTriangle(collided[i],triangle); + this->getPrimitiveTriangle(collided[i],triangle); callback->processTriangle(triangle.m_vertices,part,collided[i]); } - unlock(); + unlockChildShapes(); } void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { - GUINT i = m_mesh_parts.size(); + int i = m_mesh_parts.size(); while(i--) { m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax); diff --git a/Extras/GIMPACT/src/Bullet/btTriangleShapeEx.cpp b/Extras/GIMPACT/src/Bullet/btTriangleShapeEx.cpp new file mode 100644 index 000000000..58f38b6bf --- /dev/null +++ b/Extras/GIMPACT/src/Bullet/btTriangleShapeEx.cpp @@ -0,0 +1,216 @@ +/*! \file btGImpactTriangleShape.h +\author Francisco León Nájera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "GIMPACT/Bullet/btTriangleShapeEx.h" + + + +void BT_TRIANGLE_CONTACT::merge_points(const btVector4 & plane, + btScalar margin, const btVector3 * points, int point_count) +{ + m_point_count = 0; + m_penetration_depth= -1000.0f; + + int point_indices[MAX_TRI_CLIPPING]; + + for (int _k=0;_k=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 (int _k=0;_k0.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; + + 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; + + if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; + + return true; +} + +int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points ) +{ + // edge 0 + + btVector3 temp_points[MAX_TRI_CLIPPING]; + + + 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); + + if (clipped_count == 0) return 0; + + btVector3 temp_points1[MAX_TRI_CLIPPING]; + + + // 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; + + // edge 2 + get_edge_plane(2,edgeplane); + + clipped_count = bt_plane_clip_polygon( + edgeplane,temp_points1,clipped_count,clipped_points); + + return clipped_count; +} + +bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, BT_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 + + BT_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 + BT_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_depth0.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; + + dis1 = bt_distance_point_plane(plane1,m_vertices1[1]) - 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; + + return true; +} + + diff --git a/Extras/GIMPACT/src/core/gim_contact.cpp b/Extras/GIMPACT/src/core/gim_contact.cpp index 6f8e0751e..cd030c2b9 100755 --- a/Extras/GIMPACT/src/core/gim_contact.cpp +++ b/Extras/GIMPACT/src/core/gim_contact.cpp @@ -48,10 +48,10 @@ void gim_contact_array::merge_contacts( //fill key contacts - for (GUINT i = 0;i vertices; + + if(m_transformSubShapes) + { + + //const unsigned int *src = result.mHullIndices; + for (unsigned int i=0; isetMargin(m_compoundShape->getMargin()); + + if(m_transformSubShapes) + { + btTransform trans; + trans.setIdentity(); + trans.setOrigin(centroid); + + // add convex shape + + m_compoundShape->addChildShape(trans,convexShape); + } + else + { + btTransform trans; + trans.setIdentity(); + //trans.setOrigin(centroid); + + // add convex shape + + m_compoundShape->addChildShape(trans,convexShape); + + //m_compoundShape->addChildShape(convexShape); + } + } + + void processDecomposition(int part) + { + btGImpactMeshShapePart::TrimeshPrimitiveManager * trimeshInterface = + m_compoundShape->getTrimeshInterface(part); + + + trimeshInterface->lock(); + + //collect vertices + btAlignedObjectArray vertices; + vertices.reserve(trimeshInterface->get_vertex_count()*3); + + for(int vi = 0;viget_vertex_count();vi++) + { + btVector3 vec; + trimeshInterface->get_vertex(vi,vec); + vertices.push_back(vec[0]); + vertices.push_back(vec[1]); + vertices.push_back(vec[2]); + } + + + //collect indices + btAlignedObjectArray indices; + indices.reserve(trimeshInterface->get_primitive_count()*3); + + + for(int i = 0;iget_primitive_count();i++) + { + int i0, i1,i2; + trimeshInterface->get_indices(i,i0,i1,i2); + indices.push_back(i0); + indices.push_back(i1); + indices.push_back(i2); + } + + trimeshInterface->unlock(); + + + + unsigned int depth = 5; + float cpercent = 5; + float ppercent = 15; + unsigned int maxv = 16; + float skinWidth = 0.0; + + + ConvexDecomposition::DecompDesc desc; + desc.mVcount = trimeshInterface->get_vertex_count(); + desc.mVertices = &vertices[0]; + desc.mTcount = trimeshInterface->get_primitive_count(); + desc.mIndices = &indices[0]; + desc.mDepth = depth; + desc.mCpercent = cpercent; + desc.mPpercent = ppercent; + desc.mMaxVertices = maxv; + desc.mSkinWidth = skinWidth; + desc.mCallback = this; + + //convexDecomposition.performConvexDecomposition(desc); + + ConvexBuilder cb(desc.mCallback); + cb.process(desc); + } + + + + +}; + + + +void btGImpactConvexDecompositionShape::buildConvexDecomposition(bool transformSubShapes) +{ + GIM_ConvexDecomposition decomposition(this,transformSubShapes); + + int part_count = m_trimeshInterfaces.size(); + for (int i = 0;i(ptr); + + trimeshInterface->lock(); + + btPrimitiveTriangle triangle; + + + int i = trimeshInterface->get_primitive_count(); + while(i--) + { + trimeshInterface->get_primitive_triangle(i,triangle); + callback->processTriangle(triangle.m_vertices,part,i); + } + + trimeshInterface->unlock(); + } + + +} diff --git a/Extras/GIMPACTUtils/btGImpactConvexDecompositionShape.h b/Extras/GIMPACTUtils/btGImpactConvexDecompositionShape.h new file mode 100644 index 000000000..147f91099 --- /dev/null +++ b/Extras/GIMPACTUtils/btGImpactConvexDecompositionShape.h @@ -0,0 +1,86 @@ +/*! \file btGImpactConvexDecompositionShape.h +\author Francisco León Nájera +*/ +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@yahoo.com + + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef GIMPACT_CONVEX_DECOMPOSITION_SHAPE_H +#define GIMPACT_CONVEX_DECOMPOSITION_SHAPE_H + + +#include "GIMPACT/Bullet/btGImpactShape.h" // box tree class + + + +//! This class creates a decomposition from a trimesh. +/*! + +*/ +class btGImpactConvexDecompositionShape : public btGImpactCompoundShape +{ +protected: + btAlignedObjectArray m_trimeshInterfaces; + + void buildConvexDecomposition(bool transformSubShapes); +public: + + btGImpactConvexDecompositionShape( + btStridingMeshInterface * meshInterface, + const btVector3 & mesh_scale, + btScalar margin = btScalar(0.01),bool children_has_transform = true) + :btGImpactCompoundShape(children_has_transform) + { + + m_collisionMargin = margin; + + btGImpactMeshShapePart::TrimeshPrimitiveManager triInterface; + triInterface.m_meshInterface = meshInterface; + triInterface.m_scale = mesh_scale; + triInterface.m_margin = btScalar(1.0); + + //add parts + int part_count = meshInterface->getNumSubParts(); + for (int i=0;i< part_count;i++ ) + { + triInterface.m_part = i; + m_trimeshInterfaces.push_back(triInterface); + } + + + buildConvexDecomposition(children_has_transform); + } + + virtual ~btGImpactConvexDecompositionShape() + { + } + + SIMD_FORCE_INLINE btGImpactMeshShapePart::TrimeshPrimitiveManager * getTrimeshInterface(int part) + { + return &m_trimeshInterfaces[part]; + } + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + +}; + + + + +#endif //GIMPACT_MESH_SHAPE_H