Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

475
src/BulletCollision/Gimpact/btBoxCollision.h
View File

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

139
src/BulletCollision/Gimpact/btClipPolygon.h
View File

@@ -27,77 +27,74 @@ subject to the following restrictions:
#include "LinearMath/btTransform.h"
#include "LinearMath/btGeometryUtil.h"
SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 & plane,const btVector3 &point)
SIMD_FORCE_INLINE btScalar bt_distance_point_plane(const btVector4 &plane, const btVector3 &point)
{
return point.dot(plane) - plane[3];
}
/*! Vector blending
Takes two vectors a, b, blends them together*/
SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va,const btVector3 &vb, btScalar blend_factor)
SIMD_FORCE_INLINE void bt_vec_blend(btVector3 &vr, const btVector3 &va, const btVector3 &vb, btScalar blend_factor)
{
vr = (1-blend_factor)*va + blend_factor*vb;
vr = (1 - blend_factor) * va + blend_factor * vb;
}
//! This function calcs the distance from a 3D plane
SIMD_FORCE_INLINE void bt_plane_clip_polygon_collect(
const btVector3 & point0,
const btVector3 & point1,
btScalar dist0,
btScalar dist1,
btVector3 * clipped,
int & clipped_count)
const btVector3 &point0,
const btVector3 &point1,
btScalar dist0,
btScalar dist1,
btVector3 *clipped,
int &clipped_count)
{
bool _prevclassif = (dist0>SIMD_EPSILON);
bool _classif = (dist1>SIMD_EPSILON);
if(_classif!=_prevclassif)
bool _prevclassif = (dist0 > SIMD_EPSILON);
bool _classif = (dist1 > SIMD_EPSILON);
if (_classif != _prevclassif)
{
btScalar blendfactor = -dist0/(dist1-dist0);
bt_vec_blend(clipped[clipped_count],point0,point1,blendfactor);
btScalar blendfactor = -dist0 / (dist1 - dist0);
bt_vec_blend(clipped[clipped_count], point0, point1, blendfactor);
clipped_count++;
}
if(!_classif)
if (!_classif)
{
clipped[clipped_count] = point1;
clipped_count++;
}
}
//! Clips a polygon by a plane
/*!
*\return The count of the clipped counts
*/
SIMD_FORCE_INLINE int bt_plane_clip_polygon(
const btVector4 & plane,
const btVector3 * polygon_points,
int polygon_point_count,
btVector3 * clipped)
const btVector4 &plane,
const btVector3 *polygon_points,
int polygon_point_count,
btVector3 *clipped)
{
int clipped_count = 0;
int clipped_count = 0;
//clip first point
btScalar firstdist = bt_distance_point_plane(plane,polygon_points[0]);;
if(!(firstdist>SIMD_EPSILON))
//clip first point
btScalar firstdist = bt_distance_point_plane(plane, polygon_points[0]);
;
if (!(firstdist > SIMD_EPSILON))
{
clipped[clipped_count] = polygon_points[0];
clipped_count++;
}
btScalar olddist = firstdist;
for(int i=1;i<polygon_point_count;i++)
for (int i = 1; i < polygon_point_count; i++)
{
btScalar dist = bt_distance_point_plane(plane,polygon_points[i]);
btScalar dist = bt_distance_point_plane(plane, polygon_points[i]);
bt_plane_clip_polygon_collect(
polygon_points[i-1],polygon_points[i],
olddist,
dist,
clipped,
clipped_count);
polygon_points[i - 1], polygon_points[i],
olddist,
dist,
clipped,
clipped_count);
olddist = dist;
}
@@ -105,11 +102,11 @@ SIMD_FORCE_INLINE int bt_plane_clip_polygon(
//RETURN TO FIRST point
bt_plane_clip_polygon_collect(
polygon_points[polygon_point_count-1],polygon_points[0],
olddist,
firstdist,
clipped,
clipped_count);
polygon_points[polygon_point_count - 1], polygon_points[0],
olddist,
firstdist,
clipped,
clipped_count);
return clipped_count;
}
@@ -120,18 +117,19 @@ SIMD_FORCE_INLINE int bt_plane_clip_polygon(
*\return The count of the clipped counts
*/
SIMD_FORCE_INLINE int bt_plane_clip_triangle(
const btVector4 & plane,
const btVector3 & point0,
const btVector3 & point1,
const btVector3& point2,
btVector3 * clipped // an allocated array of 16 points at least
)
const btVector4 &plane,
const btVector3 &point0,
const btVector3 &point1,
const btVector3 &point2,
btVector3 *clipped // an allocated array of 16 points at least
)
{
int clipped_count = 0;
int clipped_count = 0;
//clip first point0
btScalar firstdist = bt_distance_point_plane(plane,point0);;
if(!(firstdist>SIMD_EPSILON))
//clip first point0
btScalar firstdist = bt_distance_point_plane(plane, point0);
;
if (!(firstdist > SIMD_EPSILON))
{
clipped[clipped_count] = point0;
clipped_count++;
@@ -139,44 +137,37 @@ SIMD_FORCE_INLINE int bt_plane_clip_triangle(
// point 1
btScalar olddist = firstdist;
btScalar dist = bt_distance_point_plane(plane,point1);
btScalar dist = bt_distance_point_plane(plane, point1);
bt_plane_clip_polygon_collect(
point0,point1,
olddist,
dist,
clipped,
clipped_count);
point0, point1,
olddist,
dist,
clipped,
clipped_count);
olddist = dist;
// point 2
dist = bt_distance_point_plane(plane,point2);
dist = bt_distance_point_plane(plane, point2);
bt_plane_clip_polygon_collect(
point1,point2,
olddist,
dist,
clipped,
clipped_count);
point1, point2,
olddist,
dist,
clipped,
clipped_count);
olddist = dist;
//RETURN TO FIRST point0
bt_plane_clip_polygon_collect(
point2,point0,
olddist,
firstdist,
clipped,
clipped_count);
point2, point0,
olddist,
firstdist,
clipped,
clipped_count);
return clipped_count;
}
#endif // GIM_TRI_COLLISION_H_INCLUDED
#endif // GIM_TRI_COLLISION_H_INCLUDED

150
src/BulletCollision/Gimpact/btCompoundFromGimpact.h
View File

@@ -5,7 +5,8 @@
#include "btGImpactShape.h"
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
ATTRIBUTE_ALIGNED16(class) btCompoundFromGimpactShape : public btCompoundShape
ATTRIBUTE_ALIGNED16(class)
btCompoundFromGimpactShape : public btCompoundShape
{
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -18,92 +19,87 @@ public:
delete m_children[i].m_childShape;
}
}
};
struct MyCallback : public btTriangleRaycastCallback
{
int m_ignorePart;
int m_ignoreTriangleIndex;
MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex)
: btTriangleRaycastCallback(from, to),
m_ignorePart(ignorePart),
m_ignoreTriangleIndex(ignoreTriangleIndex)
{
}
virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex)
{
if (partId != m_ignorePart || triangleIndex != m_ignoreTriangleIndex)
{
int m_ignorePart;
int m_ignoreTriangleIndex;
if (hitFraction < m_hitFraction)
return hitFraction;
}
MyCallback(const btVector3& from, const btVector3& to, int ignorePart, int ignoreTriangleIndex)
:btTriangleRaycastCallback(from,to),
m_ignorePart(ignorePart),
m_ignoreTriangleIndex(ignoreTriangleIndex)
{
}
virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex)
{
if (partId!=m_ignorePart || triangleIndex!=m_ignoreTriangleIndex)
{
if (hitFraction < m_hitFraction)
return hitFraction;
}
return m_hitFraction;
}
};
struct MyInternalTriangleIndexCallback : public btInternalTriangleIndexCallback
{
const btGImpactMeshShape* m_gimpactShape;
btCompoundShape* m_colShape;
btScalar m_depth;
return m_hitFraction;
}
};
struct MyInternalTriangleIndexCallback :public btInternalTriangleIndexCallback
MyInternalTriangleIndexCallback(btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth)
: m_colShape(colShape),
m_gimpactShape(meshShape),
m_depth(depth)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{
btVector3 scale = m_gimpactShape->getLocalScaling();
btVector3 v0 = triangle[0] * scale;
btVector3 v1 = triangle[1] * scale;
btVector3 v2 = triangle[2] * scale;
btVector3 centroid = (v0 + v1 + v2) / 3;
btVector3 normal = (v1 - v0).cross(v2 - v0);
normal.normalize();
btVector3 rayFrom = centroid;
btVector3 rayTo = centroid - normal * m_depth;
MyCallback cb(rayFrom, rayTo, partId, triangleIndex);
m_gimpactShape->processAllTrianglesRay(&cb, rayFrom, rayTo);
if (cb.m_hitFraction < 1)
{
const btGImpactMeshShape* m_gimpactShape;
btCompoundShape* m_colShape;
btScalar m_depth;
rayTo.setInterpolate3(cb.m_from, cb.m_to, cb.m_hitFraction);
//rayTo = cb.m_from;
//rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction);
//gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0));
}
MyInternalTriangleIndexCallback (btCompoundShape* colShape, const btGImpactMeshShape* meshShape, btScalar depth)
:m_colShape(colShape),
m_gimpactShape(meshShape),
m_depth(depth)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{
btVector3 scale = m_gimpactShape->getLocalScaling();
btVector3 v0=triangle[0]*scale;
btVector3 v1=triangle[1]*scale;
btVector3 v2=triangle[2]*scale;
btVector3 centroid = (v0+v1+v2)/3;
btVector3 normal = (v1-v0).cross(v2-v0);
normal.normalize();
btVector3 rayFrom = centroid;
btVector3 rayTo = centroid-normal*m_depth;
MyCallback cb(rayFrom,rayTo,partId,triangleIndex);
m_gimpactShape->processAllTrianglesRay(&cb,rayFrom, rayTo);
if (cb.m_hitFraction<1)
{
rayTo.setInterpolate3(cb.m_from,cb.m_to,cb.m_hitFraction);
//rayTo = cb.m_from;
//rayTo = rayTo.lerp(cb.m_to,cb.m_hitFraction);
//gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0));
}
btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0, v1, v2, rayTo);
btTransform ident;
ident.setIdentity();
m_colShape->addChildShape(ident, tet);
}
};
btBU_Simplex1to4* tet = new btBU_Simplex1to4(v0,v1,v2,rayTo);
btTransform ident;
ident.setIdentity();
m_colShape->addChildShape(ident,tet);
}
};
btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth)
btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth)
{
btCompoundShape* colShape = new btCompoundFromGimpactShape();
btTransform tr;
tr.setIdentity();
MyInternalTriangleIndexCallback cb(colShape,gimpactMesh, depth);
btVector3 aabbMin,aabbMax;
gimpactMesh->getAabb(tr,aabbMin,aabbMax);
gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb,aabbMin,aabbMax);
return colShape;
}
btTransform tr;
tr.setIdentity();
#endif //BT_COMPOUND_FROM_GIMPACT
MyInternalTriangleIndexCallback cb(colShape, gimpactMesh, depth);
btVector3 aabbMin, aabbMax;
gimpactMesh->getAabb(tr, aabbMin, aabbMax);
gimpactMesh->getMeshInterface()->InternalProcessAllTriangles(&cb, aabbMin, aabbMax);
return colShape;
}
#endif //BT_COMPOUND_FROM_GIMPACT

116
src/BulletCollision/Gimpact/btContactProcessing.cpp
View File

@@ -27,54 +27,50 @@ struct CONTACT_KEY_TOKEN
unsigned int m_key;
int m_value;
CONTACT_KEY_TOKEN()
{
}
{
}
CONTACT_KEY_TOKEN(unsigned int key,int token)
{
m_key = key;
m_value = token;
}
CONTACT_KEY_TOKEN(unsigned int key, int token)
{
m_key = key;
m_value = token;
}
CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken)
{
m_key = rtoken.m_key;
m_value = rtoken.m_value;
}
CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken)
{
m_key = rtoken.m_key;
m_value = rtoken.m_value;
}
inline bool operator <(const CONTACT_KEY_TOKEN& other) const
inline bool operator<(const CONTACT_KEY_TOKEN& other) const
{
return (m_key < other.m_key);
}
inline bool operator >(const CONTACT_KEY_TOKEN& other) const
inline bool operator>(const CONTACT_KEY_TOKEN& other) const
{
return (m_key > other.m_key);
}
};
class CONTACT_KEY_TOKEN_COMP
{
public:
bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) const
{
return ( a < b );
}
public:
bool operator()(const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b) const
{
return (a < b);
}
};
void btContactArray::merge_contacts(
const btContactArray & contacts, bool normal_contact_average)
const btContactArray& contacts, bool normal_contact_average)
{
clear();
int i;
if(contacts.size()==0) return;
if (contacts.size() == 0) return;
if(contacts.size()==1)
if (contacts.size() == 1)
{
push_back(contacts[0]);
return;
@@ -86,16 +82,16 @@ void btContactArray::merge_contacts(
//fill key contacts
for ( i = 0;i<contacts.size() ;i++ )
for (i = 0; i < contacts.size(); i++)
{
keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i));
keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(), i));
}
//sort keys
keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP());
// Merge contacts
int coincident_count=0;
int coincident_count = 0;
btVector3 coincident_normals[MAX_COINCIDENT];
unsigned int last_key = keycontacts[0].m_key;
@@ -103,56 +99,56 @@ void btContactArray::merge_contacts(
push_back(contacts[keycontacts[0].m_value]);
GIM_CONTACT * pcontact = &(*this)[0];
GIM_CONTACT* pcontact = &(*this)[0];
for( i=1;i<keycontacts.size();i++)
for (i = 1; i < keycontacts.size(); i++)
{
key = keycontacts[i].m_key;
const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
key = keycontacts[i].m_key;
const GIM_CONTACT* scontact = &contacts[keycontacts[i].m_value];
if(last_key == key)//same points
if (last_key == key) //same points
{
//merge contact
if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth) //)
{
*pcontact = *scontact;
coincident_count = 0;
coincident_count = 0;
}
else if(normal_contact_average)
else if (normal_contact_average)
{
if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
{
if(coincident_count<MAX_COINCIDENT)
{
coincident_normals[coincident_count] = scontact->m_normal;
coincident_count++;
}
}
if (btFabs(pcontact->m_depth - scontact->m_depth) < CONTACT_DIFF_EPSILON)
{
if (coincident_count < MAX_COINCIDENT)
{
coincident_normals[coincident_count] = scontact->m_normal;
coincident_count++;
}
}
}
}
else
{//add new contact
{ //add new contact
if(normal_contact_average && coincident_count>0)
{
pcontact->interpolate_normals(coincident_normals,coincident_count);
coincident_count = 0;
}
if (normal_contact_average && coincident_count > 0)
{
pcontact->interpolate_normals(coincident_normals, coincident_count);
coincident_count = 0;
}
push_back(*scontact);
pcontact = &(*this)[this->size()-1];
}
push_back(*scontact);
pcontact = &(*this)[this->size() - 1];
}
last_key = key;
}
}
void btContactArray::merge_contacts_unique(const btContactArray & contacts)
void btContactArray::merge_contacts_unique(const btContactArray& contacts)
{
clear();
if(contacts.size()==0) return;
if (contacts.size() == 0) return;
if(contacts.size()==1)
if (contacts.size() == 1)
{
push_back(contacts[0]);
return;
@@ -160,14 +156,14 @@ void btContactArray::merge_contacts_unique(const btContactArray & contacts)
GIM_CONTACT average_contact = contacts[0];
for (int i=1;i<contacts.size() ;i++ )
for (int i = 1; i < contacts.size(); i++)
{
average_contact.m_point += contacts[i].m_point;
average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
}
//divide
btScalar divide_average = 1.0f/((btScalar)contacts.size());
btScalar divide_average = 1.0f / ((btScalar)contacts.size());
average_contact.m_point *= divide_average;
@@ -176,6 +172,4 @@ void btContactArray::merge_contacts_unique(const btContactArray & contacts)
average_contact.m_depth = average_contact.m_normal.length();
average_contact.m_normal /= average_contact.m_depth;
}

20
src/BulletCollision/Gimpact/btContactProcessing.h
View File

@@ -29,7 +29,7 @@ subject to the following restrictions:
#include "btTriangleShapeEx.h"
#include "btContactProcessingStructs.h"
class btContactArray:public btAlignedObjectArray<GIM_CONTACT>
class btContactArray : public btAlignedObjectArray<GIM_CONTACT>
{
public:
btContactArray()
@@ -38,28 +38,28 @@ public:
}
SIMD_FORCE_INLINE void push_contact(
const btVector3 &point,const btVector3 & normal,
const btVector3 &point, const btVector3 &normal,
btScalar depth, int feature1, int feature2)
{
push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) );
push_back(GIM_CONTACT(point, normal, depth, feature1, feature2));
}
SIMD_FORCE_INLINE void push_triangle_contacts(
const GIM_TRIANGLE_CONTACT & tricontact,
int feature1,int feature2)
const GIM_TRIANGLE_CONTACT &tricontact,
int feature1, int feature2)
{
for(int i = 0;i<tricontact.m_point_count ;i++ )
for (int i = 0; i < tricontact.m_point_count; i++)
{
push_contact(
tricontact.m_points[i],
tricontact.m_separating_normal,
tricontact.m_penetration_depth,feature1,feature2);
tricontact.m_penetration_depth, feature1, feature2);
}
}
void merge_contacts(const btContactArray & contacts, bool normal_contact_average = true);
void merge_contacts(const btContactArray &contacts, bool normal_contact_average = true);
void merge_contacts_unique(const btContactArray & contacts);
void merge_contacts_unique(const btContactArray &contacts);
};
#endif // GIM_CONTACT_H_INCLUDED
#endif // GIM_CONTACT_H_INCLUDED

88
src/BulletCollision/Gimpact/btContactProcessingStructs.h
View File

@@ -28,7 +28,6 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h"
#include "btTriangleShapeEx.h"
/**
Configuration var for applying interpolation of contact normals
*/
@@ -41,69 +40,66 @@ Configuration var for applying interpolation of contact normals
class GIM_CONTACT
{
public:
btVector3 m_point;
btVector3 m_normal;
btScalar m_depth;//Positive value indicates interpenetration
btScalar m_distance;//Padding not for use
int m_feature1;//Face number
int m_feature2;//Face number
btVector3 m_point;
btVector3 m_normal;
btScalar m_depth; //Positive value indicates interpenetration
btScalar m_distance; //Padding not for use
int m_feature1; //Face number
int m_feature2; //Face number
public:
GIM_CONTACT()
{
}
GIM_CONTACT()
{
}
GIM_CONTACT(const GIM_CONTACT & contact):
m_point(contact.m_point),
m_normal(contact.m_normal),
m_depth(contact.m_depth),
m_feature1(contact.m_feature1),
m_feature2(contact.m_feature2)
{
}
GIM_CONTACT(const GIM_CONTACT &contact) : m_point(contact.m_point),
m_normal(contact.m_normal),
m_depth(contact.m_depth),
m_feature1(contact.m_feature1),
m_feature2(contact.m_feature2)
{
}
GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
btScalar depth, int feature1, int feature2):
m_point(point),
m_normal(normal),
m_depth(depth),
m_feature1(feature1),
m_feature2(feature2)
{
}
GIM_CONTACT(const btVector3 &point, const btVector3 &normal,
btScalar depth, int feature1, int feature2) : m_point(point),
m_normal(normal),
m_depth(depth),
m_feature1(feature1),
m_feature2(feature2)
{
}
//! Calcs key for coord classification
SIMD_FORCE_INLINE unsigned int calc_key_contact() const
{
int _coords[] = {
(int)(m_point[0]*1000.0f+1.0f),
(int)(m_point[1]*1333.0f),
(int)(m_point[2]*2133.0f+3.0f)};
unsigned int _hash=0;
SIMD_FORCE_INLINE unsigned int calc_key_contact() const
{
int _coords[] = {
(int)(m_point[0] * 1000.0f + 1.0f),
(int)(m_point[1] * 1333.0f),
(int)(m_point[2] * 2133.0f + 3.0f)};
unsigned int _hash = 0;
unsigned int *_uitmp = (unsigned int *)(&_coords[0]);
_hash = *_uitmp;
_uitmp++;
_hash += (*_uitmp)<<4;
_hash += (*_uitmp) << 4;
_uitmp++;
_hash += (*_uitmp)<<8;
_hash += (*_uitmp) << 8;
return _hash;
}
}
SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,int normal_count)
{
btVector3 vec_sum(m_normal);
for(int i=0;i<normal_count;i++)
SIMD_FORCE_INLINE void interpolate_normals(btVector3 *normals, int normal_count)
{
btVector3 vec_sum(m_normal);
for (int i = 0; i < normal_count; i++)
{
vec_sum += normals[i];
}
btScalar vec_sum_len = vec_sum.length2();
if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
if (vec_sum_len < CONTACT_DIFF_EPSILON) return;
//GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
m_normal = vec_sum/btSqrt(vec_sum_len);
}
m_normal = vec_sum / btSqrt(vec_sum_len);
}
};
#endif // BT_CONTACT_H_STRUCTS_INCLUDED
#endif // BT_CONTACT_H_STRUCTS_INCLUDED

286
src/BulletCollision/Gimpact/btGImpactBvh.cpp
View File

@@ -30,7 +30,6 @@ btClock g_tree_clock;
float g_accum_tree_collision_time = 0;
int g_count_traversing = 0;
void bt_begin_gim02_tree_time()
{
g_tree_clock.reset();
@@ -45,7 +44,7 @@ void bt_end_gim02_tree_time()
//! Gets the average time in miliseconds of tree collisions
float btGImpactBvh::getAverageTreeCollisionTime()
{
if(g_count_traversing == 0) return 0;
if (g_count_traversing == 0) return 0;
float avgtime = g_accum_tree_collision_time;
avgtime /= (float)g_count_traversing;
@@ -54,80 +53,76 @@ float btGImpactBvh::getAverageTreeCollisionTime()
g_count_traversing = 0;
return avgtime;
// float avgtime = g_count_traversing;
// g_count_traversing = 0;
// return avgtime;
// float avgtime = g_count_traversing;
// g_count_traversing = 0;
// return avgtime;
}
#endif //TRI_COLLISION_PROFILING
#endif //TRI_COLLISION_PROFILING
/////////////////////// btBvhTree /////////////////////////////////
int btBvhTree::_calc_splitting_axis(
GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
{
int i;
btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
int numIndices = endIndex-startIndex;
btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.));
int numIndices = endIndex - startIndex;
for (i=startIndex;i<endIndex;i++)
for (i = startIndex; i < endIndex; i++)
{
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
means+=center;
btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
means += center;
}
means *= (btScalar(1.)/(btScalar)numIndices);
means *= (btScalar(1.) / (btScalar)numIndices);
for (i=startIndex;i<endIndex;i++)
for (i = startIndex; i < endIndex; i++)
{
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
btVector3 diff2 = center-means;
btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
btVector3 diff2 = center - means;
diff2 = diff2 * diff2;
variance += diff2;
}
variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );
variance *= (btScalar(1.) / ((btScalar)numIndices - 1));
return variance.maxAxis();
}
int btBvhTree::_sort_and_calc_splitting_index(
GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex,
int endIndex, int splitAxis)
{
int i;
int splitIndex =startIndex;
int splitIndex = startIndex;
int numIndices = endIndex - startIndex;
// average of centers
btScalar splitValue = 0.0f;
btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
for (i=startIndex;i<endIndex;i++)
btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
for (i = startIndex; i < endIndex; i++)
{
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
means+=center;
btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
means += center;
}
means *= (btScalar(1.)/(btScalar)numIndices);
means *= (btScalar(1.) / (btScalar)numIndices);
splitValue = means[splitAxis];
//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
for (i=startIndex;i<endIndex;i++)
for (i = startIndex; i < endIndex; i++)
{
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
if (center[splitAxis] > splitValue)
{
//swap
primitive_boxes.swap(i,splitIndex);
primitive_boxes.swap(i, splitIndex);
//swapLeafNodes(i,splitIndex);
splitIndex++;
}
@@ -142,32 +137,30 @@ int btBvhTree::_sort_and_calc_splitting_index(
//bool unbalanced2 = true;
//this should be safe too:
int rangeBalancedIndices = numIndices/3;
bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
int rangeBalancedIndices = numIndices / 3;
bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
if (unbalanced)
{
splitIndex = startIndex+ (numIndices>>1);
splitIndex = startIndex + (numIndices >> 1);
}
btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex))));
return splitIndex;
}
void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
{
int curIndex = m_num_nodes;
m_num_nodes++;
btAssert((endIndex-startIndex)>0);
btAssert((endIndex - startIndex) > 0);
if ((endIndex-startIndex)==1)
if ((endIndex - startIndex) == 1)
{
//We have a leaf node
setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
//We have a leaf node
setNodeBound(curIndex, primitive_boxes[startIndex].m_bound);
m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
return;
@@ -175,47 +168,42 @@ void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startI
//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
//split axis
int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex);
splitIndex = _sort_and_calc_splitting_index(
primitive_boxes,startIndex,endIndex,
splitIndex//split axis
);
primitive_boxes, startIndex, endIndex,
splitIndex //split axis
);
//calc this node bounding box
btAABB node_bound;
node_bound.invalidate();
for (int i=startIndex;i<endIndex;i++)
for (int i = startIndex; i < endIndex; i++)
{
node_bound.merge(primitive_boxes[i].m_bound);
}
setNodeBound(curIndex,node_bound);
setNodeBound(curIndex, node_bound);
//build left branch
_build_sub_tree(primitive_boxes, startIndex, splitIndex );
_build_sub_tree(primitive_boxes, startIndex, splitIndex);
//build right branch
_build_sub_tree(primitive_boxes, splitIndex ,endIndex);
_build_sub_tree(primitive_boxes, splitIndex, endIndex);
m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
}
//! stackless build tree
void btBvhTree::build_tree(
GIM_BVH_DATA_ARRAY & primitive_boxes)
GIM_BVH_DATA_ARRAY& primitive_boxes)
{
// initialize node count to 0
m_num_nodes = 0;
// allocate nodes
m_node_array.resize(primitive_boxes.size()*2);
m_node_array.resize(primitive_boxes.size() * 2);
_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
}
@@ -225,13 +213,13 @@ void btBvhTree::build_tree(
void btGImpactBvh::refit()
{
int nodecount = getNodeCount();
while(nodecount--)
while (nodecount--)
{
if(isLeafNode(nodecount))
if (isLeafNode(nodecount))
{
btAABB leafbox;
m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
setNodeBound(nodecount,leafbox);
m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox);
setNodeBound(nodecount, leafbox);
}
else
{
@@ -243,20 +231,20 @@ void btGImpactBvh::refit()
btAABB temp_box;
int child_node = getLeftNode(nodecount);
if(child_node)
if (child_node)
{
getNodeBound(child_node,temp_box);
getNodeBound(child_node, temp_box);
bound.merge(temp_box);
}
child_node = getRightNode(nodecount);
if(child_node)
if (child_node)
{
getNodeBound(child_node,temp_box);
getNodeBound(child_node, temp_box);
bound.merge(temp_box);
}
setNodeBound(nodecount,bound);
setNodeBound(nodecount, bound);
}
}
}
@@ -268,17 +256,17 @@ void btGImpactBvh::buildSet()
GIM_BVH_DATA_ARRAY primitive_boxes;
primitive_boxes.resize(m_primitive_manager->get_primitive_count());
for (int i = 0;i<primitive_boxes.size() ;i++ )
for (int i = 0; i < primitive_boxes.size(); i++)
{
m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
primitive_boxes[i].m_data = i;
m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound);
primitive_boxes[i].m_data = i;
}
m_box_tree.build_tree(primitive_boxes);
}
//! returns the indices of the primitives in the m_primitive_manager
bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
bool btGImpactBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const
{
int curIndex = 0;
int numNodes = getNodeCount();
@@ -286,7 +274,7 @@ bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & coll
while (curIndex < numNodes)
{
btAABB bound;
getNodeBound(curIndex,bound);
getNodeBound(curIndex, bound);
//catch bugs in tree data
@@ -306,19 +294,17 @@ bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & coll
else
{
//skip node
curIndex+= getEscapeNodeIndex(curIndex);
curIndex += getEscapeNodeIndex(curIndex);
}
}
if(collided_results.size()>0) return true;
if (collided_results.size() > 0) return true;
return false;
}
//! returns the indices of the primitives in the m_primitive_manager
bool btGImpactBvh::rayQuery(
const btVector3 & ray_dir,const btVector3 & ray_origin ,
btAlignedObjectArray<int> & collided_results) const
const btVector3& ray_dir, const btVector3& ray_origin,
btAlignedObjectArray<int>& collided_results) const
{
int curIndex = 0;
int numNodes = getNodeCount();
@@ -326,16 +312,16 @@ bool btGImpactBvh::rayQuery(
while (curIndex < numNodes)
{
btAABB bound;
getNodeBound(curIndex,bound);
getNodeBound(curIndex, bound);
//catch bugs in tree data
bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir);
bool isleafnode = isLeafNode(curIndex);
if (isleafnode && aabbOverlap)
{
collided_results.push_back(getNodeData( curIndex));
collided_results.push_back(getNodeData(curIndex));
}
if (aabbOverlap || isleafnode)
@@ -346,153 +332,133 @@ bool btGImpactBvh::rayQuery(
else
{
//skip node
curIndex+= getEscapeNodeIndex(curIndex);
curIndex += getEscapeNodeIndex(curIndex);
}
}
if(collided_results.size()>0) return true;
if (collided_results.size() > 0) return true;
return false;
}
SIMD_FORCE_INLINE bool _node_collision(
btGImpactBvh * boxset0, btGImpactBvh * boxset1,
const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
int node0 ,int node1, bool complete_primitive_tests)
btGImpactBvh* boxset0, btGImpactBvh* boxset1,
const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
int node0, int node1, bool complete_primitive_tests)
{
btAABB box0;
boxset0->getNodeBound(node0,box0);
boxset0->getNodeBound(node0, box0);
btAABB box1;
boxset1->getNodeBound(node1,box1);
return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
// box1.appy_transform_trans_cache(trans_cache_1to0);
// return box0.has_collision(box1);
boxset1->getNodeBound(node1, box1);
return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests);
// box1.appy_transform_trans_cache(trans_cache_1to0);
// return box0.has_collision(box1);
}
//stackless recursive collision routine
static void _find_collision_pairs_recursive(
btGImpactBvh * boxset0, btGImpactBvh * boxset1,
btPairSet * collision_pairs,
const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
btGImpactBvh* boxset0, btGImpactBvh* boxset1,
btPairSet* collision_pairs,
const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
int node0, int node1, bool complete_primitive_tests)
{
if (_node_collision(
boxset0, boxset1, trans_cache_1to0,
node0, node1, complete_primitive_tests) == false) return; //avoid colliding internal nodes
if( _node_collision(
boxset0,boxset1,trans_cache_1to0,
node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
if(boxset0->isLeafNode(node0))
if (boxset0->isLeafNode(node0))
{
if(boxset1->isLeafNode(node1))
if (boxset1->isLeafNode(node1))
{
// collision result
collision_pairs->push_pair(
boxset0->getNodeData(node0),boxset1->getNodeData(node1));
boxset0->getNodeData(node0), boxset1->getNodeData(node1));
return;
}
else
{
//collide left recursive
_find_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
node0,boxset1->getLeftNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
node0, boxset1->getLeftNode(node1), false);
//collide right recursive
_find_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
node0,boxset1->getRightNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
node0, boxset1->getRightNode(node1), false);
}
}
else
{
if(boxset1->isLeafNode(node1))
if (boxset1->isLeafNode(node1))
{
//collide left recursive
_find_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getLeftNode(node0),node1,false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getLeftNode(node0), node1, false);
//collide right recursive
_find_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getRightNode(node0),node1,false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getRightNode(node0), node1, false);
}
else
{
//collide left0 left1
_find_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false);
//collide left0 right1
_find_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false);
//collide right0 left1
_find_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false);
//collide right0 right1
_find_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getRightNode(node0), boxset1->getRightNode(node1), false);
}// else if node1 is not a leaf
}// else if node0 is not a leaf
} // else if node1 is not a leaf
} // else if node0 is not a leaf
}
void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0,
btGImpactBvh * boxset1, const btTransform & trans1,
btPairSet & collision_pairs)
void btGImpactBvh::find_collision(btGImpactBvh* boxset0, const btTransform& trans0,
btGImpactBvh* boxset1, const btTransform& trans1,
btPairSet& collision_pairs)
{
if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return;
BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
trans_cache_1to0.calc_from_homogenic(trans0,trans1);
trans_cache_1to0.calc_from_homogenic(trans0, trans1);
#ifdef TRI_COLLISION_PROFILING
bt_begin_gim02_tree_time();
#endif //TRI_COLLISION_PROFILING
#endif //TRI_COLLISION_PROFILING
_find_collision_pairs_recursive(
boxset0,boxset1,
&collision_pairs,trans_cache_1to0,0,0,true);
boxset0, boxset1,
&collision_pairs, trans_cache_1to0, 0, 0, true);
#ifdef TRI_COLLISION_PROFILING
bt_end_gim02_tree_time();
#endif //TRI_COLLISION_PROFILING
#endif //TRI_COLLISION_PROFILING
}

105
src/BulletCollision/Gimpact/btGImpactBvh.h
View File

@@ -24,7 +24,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/btAlignedObjectArray.h"
#include "btBoxCollision.h"
@@ -32,50 +31,48 @@ subject to the following restrictions:
#include "btGImpactBvhStructs.h"
//! A pairset array
class btPairSet: public btAlignedObjectArray<GIM_PAIR>
class btPairSet : public btAlignedObjectArray<GIM_PAIR>
{
public:
btPairSet()
{
reserve(32);
}
inline void push_pair(int index1,int index2)
inline void push_pair(int index1, int index2)
{
push_back(GIM_PAIR(index1,index2));
push_back(GIM_PAIR(index1, index2));
}
inline void push_pair_inv(int index1,int index2)
inline void push_pair_inv(int index1, int index2)
{
push_back(GIM_PAIR(index2,index1));
push_back(GIM_PAIR(index2, index1));
}
};
class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray<GIM_BVH_DATA>
class GIM_BVH_DATA_ARRAY : public btAlignedObjectArray<GIM_BVH_DATA>
{
};
class GIM_BVH_TREE_NODE_ARRAY:public btAlignedObjectArray<GIM_BVH_TREE_NODE>
class GIM_BVH_TREE_NODE_ARRAY : public btAlignedObjectArray<GIM_BVH_TREE_NODE>
{
};
//! Basic Box tree structure
class btBvhTree
{
protected:
int m_num_nodes;
GIM_BVH_TREE_NODE_ARRAY m_node_array;
protected:
int _sort_and_calc_splitting_index(
GIM_BVH_DATA_ARRAY & primitive_boxes,
int startIndex, int endIndex, int splitAxis);
GIM_BVH_DATA_ARRAY& primitive_boxes,
int startIndex, int endIndex, int splitAxis);
int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex);
int _calc_splitting_axis(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex);
void _build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex);
void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex);
public:
btBvhTree()
{
@@ -84,7 +81,7 @@ public:
//! prototype functions for box tree management
//!@{
void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes);
void build_tree(GIM_BVH_DATA_ARRAY& primitive_boxes);
SIMD_FORCE_INLINE void clearNodes()
{
@@ -109,25 +106,25 @@ public:
return m_node_array[nodeindex].getDataIndex();
}
SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const
{
bound = m_node_array[nodeindex].m_bound;
}
SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound)
{
m_node_array[nodeindex].m_bound = bound;
}
SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
{
return nodeindex+1;
return nodeindex + 1;
}
SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
{
if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
if (m_node_array[nodeindex + 1].isLeafNode()) return nodeindex + 2;
return nodeindex + 1 + m_node_array[nodeindex + 1].getEscapeIndex();
}
SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
@@ -135,7 +132,7 @@ public:
return m_node_array[nodeindex].getEscapeIndex();
}
SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const
SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE* get_node_pointer(int index = 0) const
{
return &m_node_array[index];
}
@@ -143,7 +140,6 @@ public:
//!@}
};
//! Prototype Base class for primitive classification
/*!
This class is a wrapper for primitive collections.
@@ -153,18 +149,16 @@ This class can manage Compound shapes and trimeshes, and if it is managing trime
class btPrimitiveManagerBase
{
public:
virtual ~btPrimitiveManagerBase() {}
//! determines if this manager consist on only triangles, which special case will be optimized
virtual bool is_trimesh() const = 0;
virtual int get_primitive_count() const = 0;
virtual void get_primitive_box(int prim_index ,btAABB & primbox) const = 0;
virtual void get_primitive_box(int prim_index, btAABB& primbox) const = 0;
//! retrieves only the points of the triangle, and the collision margin
virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const= 0;
virtual void get_primitive_triangle(int prim_index, btPrimitiveTriangle& triangle) const = 0;
};
//! Structure for containing Boxes
/*!
This class offers an structure for managing a box tree of primitives.
@@ -174,13 +168,13 @@ class btGImpactBvh
{
protected:
btBvhTree m_box_tree;
btPrimitiveManagerBase * m_primitive_manager;
btPrimitiveManagerBase* m_primitive_manager;
protected:
//stackless refit
void refit();
public:
public:
//! this constructor doesn't build the tree. you must call buildSet
btGImpactBvh()
{
@@ -188,31 +182,30 @@ public:
}
//! this constructor doesn't build the tree. you must call buildSet
btGImpactBvh(btPrimitiveManagerBase * primitive_manager)
btGImpactBvh(btPrimitiveManagerBase* primitive_manager)
{
m_primitive_manager = primitive_manager;
}
SIMD_FORCE_INLINE btAABB getGlobalBox() const
SIMD_FORCE_INLINE btAABB getGlobalBox() const
{
btAABB totalbox;
getNodeBound(0, totalbox);
return totalbox;
}
SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase* primitive_manager)
{
m_primitive_manager = primitive_manager;
}
SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
SIMD_FORCE_INLINE btPrimitiveManagerBase* getPrimitiveManager() const
{
return m_primitive_manager;
}
//! node manager prototype functions
///@{
//! node manager prototype functions
///@{
//! this attemps to refit the box set.
SIMD_FORCE_INLINE void update()
@@ -224,21 +217,21 @@ public:
void buildSet();
//! returns the indices of the primitives in the m_primitive_manager
bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;
bool boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const;
//! returns the indices of the primitives in the m_primitive_manager
SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box,
const btTransform & transform, btAlignedObjectArray<int> & collided_results) const
SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB& box,
const btTransform& transform, btAlignedObjectArray<int>& collided_results) const
{
btAABB transbox=box;
btAABB transbox = box;
transbox.appy_transform(transform);
return boxQuery(transbox,collided_results);
return boxQuery(transbox, collided_results);
}
//! returns the indices of the primitives in the m_primitive_manager
bool rayQuery(
const btVector3 & ray_dir,const btVector3 & ray_origin ,
btAlignedObjectArray<int> & collided_results) const;
const btVector3& ray_dir, const btVector3& ray_origin,
btAlignedObjectArray<int>& collided_results) const;
//! tells if this set has hierarcht
SIMD_FORCE_INLINE bool hasHierarchy() const
@@ -247,7 +240,7 @@ public:
}
//! tells if this set is a trimesh
SIMD_FORCE_INLINE bool isTrimesh() const
SIMD_FORCE_INLINE bool isTrimesh() const
{
return m_primitive_manager->is_trimesh();
}
@@ -269,17 +262,16 @@ public:
return m_box_tree.getNodeData(nodeindex);
}
SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const
{
m_box_tree.getNodeBound(nodeindex, bound);
}
SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound)
{
m_box_tree.setNodeBound(nodeindex, bound);
}
SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
{
return m_box_tree.getLeftNode(nodeindex);
@@ -295,24 +287,23 @@ public:
return m_box_tree.getEscapeNodeIndex(nodeindex);
}
SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex, btPrimitiveTriangle& triangle) const
{
m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex), triangle);
}
SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const
SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE* get_node_pointer(int index = 0) const
{
return m_box_tree.get_node_pointer(index);
}
#ifdef TRI_COLLISION_PROFILING
static float getAverageTreeCollisionTime();
#endif //TRI_COLLISION_PROFILING
#endif //TRI_COLLISION_PROFILING
static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1,
btGImpactBvh * boxset2, const btTransform & trans2,
btPairSet & collision_pairs);
static void find_collision(btGImpactBvh* boxset1, const btTransform& trans1,
btGImpactBvh* boxset2, const btTransform& trans2,
btPairSet& collision_pairs);
};
#endif // GIM_BOXPRUNING_H_INCLUDED
#endif // GIM_BOXPRUNING_H_INCLUDED

33
src/BulletCollision/Gimpact/btGImpactBvhStructs.h
View File

@@ -24,7 +24,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/btAlignedObjectArray.h"
#include "btBoxCollision.h"
@@ -33,21 +32,22 @@ subject to the following restrictions:
//! Overlapping pair
struct GIM_PAIR
{
int m_index1;
int m_index2;
GIM_PAIR()
{}
int m_index1;
int m_index2;
GIM_PAIR()
{
}
GIM_PAIR(const GIM_PAIR & p)
{
m_index1 = p.m_index1;
m_index2 = p.m_index2;
GIM_PAIR(const GIM_PAIR& p)
{
m_index1 = p.m_index1;
m_index2 = p.m_index2;
}
GIM_PAIR(int index1, int index2)
{
m_index1 = index1;
m_index2 = index2;
{
m_index1 = index1;
m_index2 = index2;
}
};
@@ -63,8 +63,10 @@ class GIM_BVH_TREE_NODE
{
public:
btAABB m_bound;
protected:
int m_escapeIndexOrDataIndex;
int m_escapeIndexOrDataIndex;
public:
GIM_BVH_TREE_NODE()
{
@@ -74,7 +76,7 @@ public:
SIMD_FORCE_INLINE bool isLeafNode() const
{
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
return (m_escapeIndexOrDataIndex>=0);
return (m_escapeIndexOrDataIndex >= 0);
}
SIMD_FORCE_INLINE int getEscapeIndex() const
@@ -99,7 +101,6 @@ public:
{
m_escapeIndexOrDataIndex = index;
}
};
#endif // GIM_BOXPRUNING_H_INCLUDED
#endif // GIM_BOXPRUNING_H_INCLUDED

619
src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp
View File

File diff suppressed because it is too large Load Diff

246
src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h
View File

@@ -42,7 +42,6 @@ class btDispatcher;
#include "LinearMath/btIDebugDraw.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
//! Collision Algorithm for GImpact Shapes
/*!
For register this algorithm in Bullet, proceed as following:
@@ -54,36 +53,35 @@ btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);
class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
protected:
btCollisionAlgorithm * m_convex_algorithm;
btPersistentManifold * m_manifoldPtr;
btCollisionAlgorithm* m_convex_algorithm;
btPersistentManifold* m_manifoldPtr;
btManifoldResult* m_resultOut;
const btDispatcherInfo * m_dispatchInfo;
const btDispatcherInfo* m_dispatchInfo;
int m_triface0;
int m_part0;
int m_triface1;
int m_part1;
//! Creates a new contact point
SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(const btCollisionObject* body0,const btCollisionObject* body1)
SIMD_FORCE_INLINE btPersistentManifold* newContactManifold(const btCollisionObject* body0, const btCollisionObject* body1)
{
m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
m_manifoldPtr = m_dispatcher->getNewManifold(body0, body1);
return m_manifoldPtr;
}
SIMD_FORCE_INLINE void destroyConvexAlgorithm()
{
if(m_convex_algorithm)
if (m_convex_algorithm)
{
m_convex_algorithm->~btCollisionAlgorithm();
m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm);
m_dispatcher->freeCollisionAlgorithm(m_convex_algorithm);
m_convex_algorithm = NULL;
}
}
SIMD_FORCE_INLINE void destroyContactManifolds()
{
if(m_manifoldPtr == NULL) return;
if (m_manifoldPtr == NULL) return;
m_dispatcher->releaseManifold(m_manifoldPtr);
m_manifoldPtr = NULL;
}
@@ -104,207 +102,187 @@ protected:
return m_manifoldPtr;
}
// Call before process collision
SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
if(getLastManifold() == 0)
if (getLastManifold() == 0)
{
newContactManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
newContactManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
}
m_resultOut->setPersistentManifold(getLastManifold());
}
// Call before process collision
SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
SIMD_FORCE_INLINE btCollisionAlgorithm* newAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
checkManifold(body0Wrap,body1Wrap);
checkManifold(body0Wrap, body1Wrap);
btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm(
body0Wrap,body1Wrap,getLastManifold(), BT_CONTACT_POINT_ALGORITHMS);
return convex_algorithm ;
btCollisionAlgorithm* convex_algorithm = m_dispatcher->findAlgorithm(
body0Wrap, body1Wrap, getLastManifold(), BT_CONTACT_POINT_ALGORITHMS);
return convex_algorithm;
}
// Call before process collision
SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
if(m_convex_algorithm) return;
m_convex_algorithm = newAlgorithm(body0Wrap,body1Wrap);
if (m_convex_algorithm) return;
m_convex_algorithm = newAlgorithm(body0Wrap, body1Wrap);
}
void addContactPoint(const btCollisionObjectWrapper* body0Wrap,
const btCollisionObjectWrapper* body1Wrap,
const btVector3& point,
const btVector3& normal,
btScalar distance);
void addContactPoint(const btCollisionObjectWrapper * body0Wrap,
const btCollisionObjectWrapper * body1Wrap,
const btVector3 & point,
const btVector3 & normal,
btScalar distance);
//! Collision routines
//!@{
//! Collision routines
//!@{
void collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap,
const btCollisionObjectWrapper* body1Wrap,
const btGImpactMeshShapePart * shape0,
const btGImpactMeshShapePart * shape1,
const int * pairs, int pair_count);
const btCollisionObjectWrapper* body1Wrap,
const btGImpactMeshShapePart* shape0,
const btGImpactMeshShapePart* shape1,
const int* pairs, int pair_count);
void collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap,
const btCollisionObjectWrapper* body1Wrap,
const btGImpactMeshShapePart * shape0,
const btGImpactMeshShapePart * shape1,
const int * pairs, int pair_count);
const btCollisionObjectWrapper* body1Wrap,
const btGImpactMeshShapePart* shape0,
const btGImpactMeshShapePart* shape1,
const int* pairs, int pair_count);
void shape_vs_shape_collision(
const btCollisionObjectWrapper* body0,
const btCollisionObjectWrapper* body1,
const btCollisionShape * shape0,
const btCollisionShape * shape1);
const btCollisionObjectWrapper* body0,
const btCollisionObjectWrapper* body1,
const btCollisionShape* shape0,
const btCollisionShape* shape1);
void convex_vs_convex_collision(const btCollisionObjectWrapper* body0Wrap,
const btCollisionObjectWrapper* body1Wrap,
const btCollisionShape* shape0,
const btCollisionShape* shape1);
const btCollisionObjectWrapper* body1Wrap,
const btCollisionShape* shape0,
const btCollisionShape* shape1);
void gimpact_vs_gimpact_find_pairs(
const btTransform & trans0,
const btTransform & trans1,
const btGImpactShapeInterface * shape0,
const btGImpactShapeInterface * shape1,btPairSet & pairset);
const btTransform& trans0,
const btTransform& trans1,
const btGImpactShapeInterface* shape0,
const btGImpactShapeInterface* shape1, btPairSet& pairset);
void gimpact_vs_shape_find_pairs(
const btTransform & trans0,
const btTransform & trans1,
const btGImpactShapeInterface * shape0,
const btCollisionShape * shape1,
btAlignedObjectArray<int> & collided_primitives);
const btTransform& trans0,
const btTransform& trans1,
const btGImpactShapeInterface* shape0,
const btCollisionShape* shape1,
btAlignedObjectArray<int>& collided_primitives);
void gimpacttrimeshpart_vs_plane_collision(
const btCollisionObjectWrapper * body0Wrap,
const btCollisionObjectWrapper * body1Wrap,
const btGImpactMeshShapePart * shape0,
const btStaticPlaneShape * shape1,bool swapped);
const btCollisionObjectWrapper* body0Wrap,
const btCollisionObjectWrapper* body1Wrap,
const btGImpactMeshShapePart* shape0,
const btStaticPlaneShape* shape1, bool swapped);
public:
btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap);
btGImpactCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap);
virtual ~btGImpactCollisionAlgorithm();
virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr)
manifoldArray.push_back(m_manifoldPtr);
}
btManifoldResult* internalGetResultOut()
btManifoldResult* internalGetResultOut()
{
return m_resultOut;
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
struct CreateFunc : public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm));
return new(mem) btGImpactCollisionAlgorithm(ci,body0Wrap,body1Wrap);
return new (mem) btGImpactCollisionAlgorithm(ci, body0Wrap, body1Wrap);
}
};
//! Use this function for register the algorithm externally
static void registerAlgorithm(btCollisionDispatcher * dispatcher);
static void registerAlgorithm(btCollisionDispatcher* dispatcher);
#ifdef TRI_COLLISION_PROFILING
//! Gets the average time in miliseconds of tree collisions
static float getAverageTreeCollisionTime();
//! Gets the average time in miliseconds of triangle collisions
static float getAverageTriangleCollisionTime();
#endif //TRI_COLLISION_PROFILING
#endif //TRI_COLLISION_PROFILING
//! Collides two gimpact shapes
/*!
\pre shape0 and shape1 couldn't be btGImpactMeshShape objects
*/
void gimpact_vs_gimpact(const btCollisionObjectWrapper* body0Wrap,
const btCollisionObjectWrapper * body1Wrap,
const btGImpactShapeInterface * shape0,
const btGImpactShapeInterface * shape1);
const btCollisionObjectWrapper* body1Wrap,
const btGImpactShapeInterface* shape0,
const btGImpactShapeInterface* shape1);
void gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap,
const btCollisionObjectWrapper* body1Wrap,
const btGImpactShapeInterface * shape0,
const btCollisionShape * shape1,bool swapped);
const btCollisionObjectWrapper* body1Wrap,
const btGImpactShapeInterface* shape0,
const btCollisionShape* shape1, bool swapped);
void gimpact_vs_compoundshape(const btCollisionObjectWrapper * body0Wrap,
const btCollisionObjectWrapper * body1Wrap,
const btGImpactShapeInterface * shape0,
const btCompoundShape * shape1,bool swapped);
void gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap,
const btCollisionObjectWrapper* body1Wrap,
const btGImpactShapeInterface* shape0,
const btCompoundShape* shape1, bool swapped);
void gimpact_vs_concave(
const btCollisionObjectWrapper * body0Wrap,
const btCollisionObjectWrapper * body1Wrap,
const btGImpactShapeInterface * shape0,
const btConcaveShape * shape1,bool swapped);
/// Accessor/Mutator pairs for Part and triangleID
void setFace0(int value)
{
m_triface0 = value;
}
int getFace0()
{
return m_triface0;
}
void setFace1(int value)
{
m_triface1 = value;
}
int getFace1()
{
return m_triface1;
}
void setPart0(int value)
{
m_part0 = value;
}
int getPart0()
{
return m_part0;
}
void setPart1(int value)
{
m_part1 = value;
}
int getPart1()
{
return m_part1;
}
const btCollisionObjectWrapper* body0Wrap,
const btCollisionObjectWrapper* body1Wrap,
const btGImpactShapeInterface* shape0,
const btConcaveShape* shape1, bool swapped);
/// Accessor/Mutator pairs for Part and triangleID
void setFace0(int value)
{
m_triface0 = value;
}
int getFace0()
{
return m_triface0;
}
void setFace1(int value)
{
m_triface1 = value;
}
int getFace1()
{
return m_triface1;
}
void setPart0(int value)
{
m_part0 = value;
}
int getPart0()
{
return m_part0;
}
void setPart1(int value)
{
m_part1 = value;
}
int getPart1()
{
return m_part1;
}
};
//algorithm details
//#define BULLET_TRIANGLE_COLLISION 1
#define GIMPACT_VS_PLANE_COLLISION 1
#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H
#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H

34
src/BulletCollision/Gimpact/btGImpactMassUtil.h
View File

@@ -21,40 +21,36 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef GIMPACT_MASS_UTIL_H
#define GIMPACT_MASS_UTIL_H
#include "LinearMath/btTransform.h"
SIMD_FORCE_INLINE btVector3 gim_inertia_add_transformed(
const btVector3 & source_inertia, const btVector3 & added_inertia, const btTransform & transform)
const btVector3& source_inertia, const btVector3& added_inertia, const btTransform& transform)
{
btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose();
btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose();
btScalar x2 = transform.getOrigin()[0];
x2*= x2;
x2 *= x2;
btScalar y2 = transform.getOrigin()[1];
y2*= y2;
y2 *= y2;
btScalar z2 = transform.getOrigin()[2];
z2*= z2;
z2 *= z2;
btScalar ix = rotatedTensor[0][0]*(y2+z2);
btScalar iy = rotatedTensor[1][1]*(x2+z2);
btScalar iz = rotatedTensor[2][2]*(x2+y2);
btScalar ix = rotatedTensor[0][0] * (y2 + z2);
btScalar iy = rotatedTensor[1][1] * (x2 + z2);
btScalar iz = rotatedTensor[2][2] * (x2 + y2);
return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz);
return btVector3(source_inertia[0] + ix, source_inertia[1] + iy, source_inertia[2] + iz);
}
SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3 & point, btScalar mass)
SIMD_FORCE_INLINE btVector3 gim_get_point_inertia(const btVector3& point, btScalar mass)
{
btScalar x2 = point[0]*point[0];
btScalar y2 = point[1]*point[1];
btScalar z2 = point[2]*point[2];
return btVector3(mass*(y2+z2),mass*(x2+z2),mass*(x2+y2));
btScalar x2 = point[0] * point[0];
btScalar y2 = point[1] * point[1];
btScalar z2 = point[2] * point[2];
return btVector3(mass * (y2 + z2), mass * (x2 + z2), mass * (x2 + y2));
}
#endif //GIMPACT_MESH_SHAPE_H
#endif //GIMPACT_MESH_SHAPE_H

304
src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
View File

@@ -27,11 +27,9 @@ subject to the following restrictions:
#ifdef TRI_COLLISION_PROFILING
btClock g_q_tree_clock;
float g_q_accum_tree_collision_time = 0;
int g_q_count_traversing = 0;
void bt_begin_gim02_q_tree_time()
{
g_q_tree_clock.reset();
@@ -43,11 +41,10 @@ void bt_end_gim02_q_tree_time()
g_q_count_traversing++;
}
//! Gets the average time in miliseconds of tree collisions
float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
{
if(g_q_count_traversing == 0) return 0;
if (g_q_count_traversing == 0) return 0;
float avgtime = g_q_accum_tree_collision_time;
avgtime /= (float)g_q_count_traversing;
@@ -56,99 +53,92 @@ float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
g_q_count_traversing = 0;
return avgtime;
// float avgtime = g_q_count_traversing;
// g_q_count_traversing = 0;
// return avgtime;
// float avgtime = g_q_count_traversing;
// g_q_count_traversing = 0;
// return avgtime;
}
#endif //TRI_COLLISION_PROFILING
#endif //TRI_COLLISION_PROFILING
/////////////////////// btQuantizedBvhTree /////////////////////////////////
void btQuantizedBvhTree::calc_quantization(
GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin)
GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin)
{
//calc globa box
btAABB global_bound;
global_bound.invalidate();
for (int i=0;i<primitive_boxes.size() ;i++ )
for (int i = 0; i < primitive_boxes.size(); i++)
{
global_bound.merge(primitive_boxes[i].m_bound);
}
bt_calc_quantization_parameters(
m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin);
m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization, global_bound.m_min, global_bound.m_max, boundMargin);
}
int btQuantizedBvhTree::_calc_splitting_axis(
GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
{
int i;
btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
int numIndices = endIndex-startIndex;
btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.));
int numIndices = endIndex - startIndex;
for (i=startIndex;i<endIndex;i++)
for (i = startIndex; i < endIndex; i++)
{
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
means+=center;
btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
means += center;
}
means *= (btScalar(1.)/(btScalar)numIndices);
means *= (btScalar(1.) / (btScalar)numIndices);
for (i=startIndex;i<endIndex;i++)
for (i = startIndex; i < endIndex; i++)
{
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
btVector3 diff2 = center-means;
btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
btVector3 diff2 = center - means;
diff2 = diff2 * diff2;
variance += diff2;
}
variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );
variance *= (btScalar(1.) / ((btScalar)numIndices - 1));
return variance.maxAxis();
}
int btQuantizedBvhTree::_sort_and_calc_splitting_index(
GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex,
int endIndex, int splitAxis)
{
int i;
int splitIndex =startIndex;
int splitIndex = startIndex;
int numIndices = endIndex - startIndex;
// average of centers
btScalar splitValue = 0.0f;
btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
for (i=startIndex;i<endIndex;i++)
btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
for (i = startIndex; i < endIndex; i++)
{
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
means+=center;
btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
means += center;
}
means *= (btScalar(1.)/(btScalar)numIndices);
means *= (btScalar(1.) / (btScalar)numIndices);
splitValue = means[splitAxis];
//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
for (i=startIndex;i<endIndex;i++)
for (i = startIndex; i < endIndex; i++)
{
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
if (center[splitAxis] > splitValue)
{
//swap
primitive_boxes.swap(i,splitIndex);
primitive_boxes.swap(i, splitIndex);
//swapLeafNodes(i,splitIndex);
splitIndex++;
}
@@ -163,32 +153,30 @@ int btQuantizedBvhTree::_sort_and_calc_splitting_index(
//bool unbalanced2 = true;
//this should be safe too:
int rangeBalancedIndices = numIndices/3;
bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
int rangeBalancedIndices = numIndices / 3;
bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
if (unbalanced)
{
splitIndex = startIndex+ (numIndices>>1);
splitIndex = startIndex + (numIndices >> 1);
}
btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex))));
return splitIndex;
}
void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
{
int curIndex = m_num_nodes;
m_num_nodes++;
btAssert((endIndex-startIndex)>0);
btAssert((endIndex - startIndex) > 0);
if ((endIndex-startIndex)==1)
if ((endIndex - startIndex) == 1)
{
//We have a leaf node
setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
//We have a leaf node
setNodeBound(curIndex, primitive_boxes[startIndex].m_bound);
m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
return;
@@ -196,48 +184,43 @@ void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, i
//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
//split axis
int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex);
splitIndex = _sort_and_calc_splitting_index(
primitive_boxes,startIndex,endIndex,
splitIndex//split axis
);
primitive_boxes, startIndex, endIndex,
splitIndex //split axis
);
//calc this node bounding box
btAABB node_bound;
node_bound.invalidate();
for (int i=startIndex;i<endIndex;i++)
for (int i = startIndex; i < endIndex; i++)
{
node_bound.merge(primitive_boxes[i].m_bound);
}
setNodeBound(curIndex,node_bound);
setNodeBound(curIndex, node_bound);
//build left branch
_build_sub_tree(primitive_boxes, startIndex, splitIndex );
_build_sub_tree(primitive_boxes, startIndex, splitIndex);
//build right branch
_build_sub_tree(primitive_boxes, splitIndex ,endIndex);
_build_sub_tree(primitive_boxes, splitIndex, endIndex);
m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
}
//! stackless build tree
void btQuantizedBvhTree::build_tree(
GIM_BVH_DATA_ARRAY & primitive_boxes)
GIM_BVH_DATA_ARRAY& primitive_boxes)
{
calc_quantization(primitive_boxes);
// initialize node count to 0
m_num_nodes = 0;
// allocate nodes
m_node_array.resize(primitive_boxes.size()*2);
m_node_array.resize(primitive_boxes.size() * 2);
_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
}
@@ -247,13 +230,13 @@ void btQuantizedBvhTree::build_tree(
void btGImpactQuantizedBvh::refit()
{
int nodecount = getNodeCount();
while(nodecount--)
while (nodecount--)
{
if(isLeafNode(nodecount))
if (isLeafNode(nodecount))
{
btAABB leafbox;
m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
setNodeBound(nodecount,leafbox);
m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox);
setNodeBound(nodecount, leafbox);
}
else
{
@@ -265,20 +248,20 @@ void btGImpactQuantizedBvh::refit()
btAABB temp_box;
int child_node = getLeftNode(nodecount);
if(child_node)
if (child_node)
{
getNodeBound(child_node,temp_box);
getNodeBound(child_node, temp_box);
bound.merge(temp_box);
}
child_node = getRightNode(nodecount);
if(child_node)
if (child_node)
{
getNodeBound(child_node,temp_box);
getNodeBound(child_node, temp_box);
bound.merge(temp_box);
}
setNodeBound(nodecount,bound);
setNodeBound(nodecount, bound);
}
}
}
@@ -290,17 +273,17 @@ void btGImpactQuantizedBvh::buildSet()
GIM_BVH_DATA_ARRAY primitive_boxes;
primitive_boxes.resize(m_primitive_manager->get_primitive_count());
for (int i = 0;i<primitive_boxes.size() ;i++ )
for (int i = 0; i < primitive_boxes.size(); i++)
{
m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
primitive_boxes[i].m_data = i;
m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound);
primitive_boxes[i].m_data = i;
}
m_box_tree.build_tree(primitive_boxes);
}
//! returns the indices of the primitives in the m_primitive_manager
bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
bool btGImpactQuantizedBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const
{
int curIndex = 0;
int numNodes = getNodeCount();
@@ -310,16 +293,14 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in
unsigned short quantizedMin[3];
unsigned short quantizedMax[3];
m_box_tree.quantizePoint(quantizedMin,box.m_min);
m_box_tree.quantizePoint(quantizedMax,box.m_max);
m_box_tree.quantizePoint(quantizedMin, box.m_min);
m_box_tree.quantizePoint(quantizedMax, box.m_max);
while (curIndex < numNodes)
{
//catch bugs in tree data
bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax);
bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin, quantizedMax);
bool isleafnode = isLeafNode(curIndex);
if (isleafnode && aabbOverlap)
@@ -335,19 +316,17 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in
else
{
//skip node
curIndex+= getEscapeNodeIndex(curIndex);
curIndex += getEscapeNodeIndex(curIndex);
}
}
if(collided_results.size()>0) return true;
if (collided_results.size() > 0) return true;
return false;
}
//! returns the indices of the primitives in the m_primitive_manager
bool btGImpactQuantizedBvh::rayQuery(
const btVector3 & ray_dir,const btVector3 & ray_origin ,
btAlignedObjectArray<int> & collided_results) const
const btVector3& ray_dir, const btVector3& ray_origin,
btAlignedObjectArray<int>& collided_results) const
{
int curIndex = 0;
int numNodes = getNodeCount();
@@ -355,16 +334,16 @@ bool btGImpactQuantizedBvh::rayQuery(
while (curIndex < numNodes)
{
btAABB bound;
getNodeBound(curIndex,bound);
getNodeBound(curIndex, bound);
//catch bugs in tree data
bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir);
bool isleafnode = isLeafNode(curIndex);
if (isleafnode && aabbOverlap)
{
collided_results.push_back(getNodeData( curIndex));
collided_results.push_back(getNodeData(curIndex));
}
if (aabbOverlap || isleafnode)
@@ -375,154 +354,133 @@ bool btGImpactQuantizedBvh::rayQuery(
else
{
//skip node
curIndex+= getEscapeNodeIndex(curIndex);
curIndex += getEscapeNodeIndex(curIndex);
}
}
if(collided_results.size()>0) return true;
if (collided_results.size() > 0) return true;
return false;
}
SIMD_FORCE_INLINE bool _quantized_node_collision(
const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1,
const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
int node0 ,int node1, bool complete_primitive_tests)
const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1,
const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
int node0, int node1, bool complete_primitive_tests)
{
btAABB box0;
boxset0->getNodeBound(node0,box0);
boxset0->getNodeBound(node0, box0);
btAABB box1;
boxset1->getNodeBound(node1,box1);
return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
// box1.appy_transform_trans_cache(trans_cache_1to0);
// return box0.has_collision(box1);
boxset1->getNodeBound(node1, box1);
return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests);
// box1.appy_transform_trans_cache(trans_cache_1to0);
// return box0.has_collision(box1);
}
//stackless recursive collision routine
static void _find_quantized_collision_pairs_recursive(
const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1,
btPairSet * collision_pairs,
const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1,
btPairSet* collision_pairs,
const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
int node0, int node1, bool complete_primitive_tests)
{
if (_quantized_node_collision(
boxset0, boxset1, trans_cache_1to0,
node0, node1, complete_primitive_tests) == false) return; //avoid colliding internal nodes
if( _quantized_node_collision(
boxset0,boxset1,trans_cache_1to0,
node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
if(boxset0->isLeafNode(node0))
if (boxset0->isLeafNode(node0))
{
if(boxset1->isLeafNode(node1))
if (boxset1->isLeafNode(node1))
{
// collision result
collision_pairs->push_pair(
boxset0->getNodeData(node0),boxset1->getNodeData(node1));
boxset0->getNodeData(node0), boxset1->getNodeData(node1));
return;
}
else
{
//collide left recursive
_find_quantized_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
node0,boxset1->getLeftNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
node0, boxset1->getLeftNode(node1), false);
//collide right recursive
_find_quantized_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
node0,boxset1->getRightNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
node0, boxset1->getRightNode(node1), false);
}
}
else
{
if(boxset1->isLeafNode(node1))
if (boxset1->isLeafNode(node1))
{
//collide left recursive
_find_quantized_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getLeftNode(node0),node1,false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getLeftNode(node0), node1, false);
//collide right recursive
_find_quantized_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getRightNode(node0),node1,false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getRightNode(node0), node1, false);
}
else
{
//collide left0 left1
_find_quantized_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false);
//collide left0 right1
_find_quantized_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false);
//collide right0 left1
_find_quantized_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false);
//collide right0 right1
_find_quantized_collision_pairs_recursive(
boxset0,boxset1,
collision_pairs,trans_cache_1to0,
boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
boxset0, boxset1,
collision_pairs, trans_cache_1to0,
boxset0->getRightNode(node0), boxset1->getRightNode(node1), false);
}// else if node1 is not a leaf
}// else if node0 is not a leaf
} // else if node1 is not a leaf
} // else if node0 is not a leaf
}
void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
const btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
btPairSet & collision_pairs)
void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh* boxset0, const btTransform& trans0,
const btGImpactQuantizedBvh* boxset1, const btTransform& trans1,
btPairSet& collision_pairs)
{
if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return;
BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
trans_cache_1to0.calc_from_homogenic(trans0,trans1);
trans_cache_1to0.calc_from_homogenic(trans0, trans1);
#ifdef TRI_COLLISION_PROFILING
bt_begin_gim02_q_tree_time();
#endif //TRI_COLLISION_PROFILING
#endif //TRI_COLLISION_PROFILING
_find_quantized_collision_pairs_recursive(
boxset0,boxset1,
&collision_pairs,trans_cache_1to0,0,0,true);
boxset0, boxset1,
&collision_pairs, trans_cache_1to0, 0, 0, true);
#ifdef TRI_COLLISION_PROFILING
bt_end_gim02_q_tree_time();
#endif //TRI_COLLISION_PROFILING
#endif //TRI_COLLISION_PROFILING
}

121
src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h
View File

@@ -28,13 +28,10 @@ subject to the following restrictions:
#include "btQuantization.h"
#include "btGImpactQuantizedBvhStructs.h"
class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE>
class GIM_QUANTIZED_BVH_NODE_ARRAY : public btAlignedObjectArray<BT_QUANTIZED_BVH_NODE>
{
};
//! Basic Box tree structure
class btQuantizedBvhTree
{
@@ -43,16 +40,18 @@ protected:
GIM_QUANTIZED_BVH_NODE_ARRAY m_node_array;
btAABB m_global_bound;
btVector3 m_bvhQuantization;
protected:
void calc_quantization(GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin = btScalar(1.0) );
void calc_quantization(GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin = btScalar(1.0));
int _sort_and_calc_splitting_index(
GIM_BVH_DATA_ARRAY & primitive_boxes,
int startIndex, int endIndex, int splitAxis);
GIM_BVH_DATA_ARRAY& primitive_boxes,
int startIndex, int endIndex, int splitAxis);
int _calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex);
int _calc_splitting_axis(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex);
void _build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex);
void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex);
public:
btQuantizedBvhTree()
{
@@ -61,20 +60,19 @@ public:
//! prototype functions for box tree management
//!@{
void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes);
void build_tree(GIM_BVH_DATA_ARRAY& primitive_boxes);
SIMD_FORCE_INLINE void quantizePoint(
unsigned short * quantizedpoint, const btVector3 & point) const
unsigned short* quantizedpoint, const btVector3& point) const
{
bt_quantize_clamp(quantizedpoint,point,m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization);
bt_quantize_clamp(quantizedpoint, point, m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization);
}
SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
int node_index,
unsigned short * quantizedMin,unsigned short * quantizedMax) const
unsigned short* quantizedMin, unsigned short* quantizedMax) const
{
return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax);
return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin, quantizedMax);
}
SIMD_FORCE_INLINE void clearNodes()
@@ -100,41 +98,41 @@ public:
return m_node_array[nodeindex].getDataIndex();
}
SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const
{
bound.m_min = bt_unquantize(
m_node_array[nodeindex].m_quantizedAabbMin,
m_global_bound.m_min,m_bvhQuantization);
m_global_bound.m_min, m_bvhQuantization);
bound.m_max = bt_unquantize(
m_node_array[nodeindex].m_quantizedAabbMax,
m_global_bound.m_min,m_bvhQuantization);
m_global_bound.m_min, m_bvhQuantization);
}
SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound)
{
bt_quantize_clamp( m_node_array[nodeindex].m_quantizedAabbMin,
bound.m_min,
m_global_bound.m_min,
m_global_bound.m_max,
m_bvhQuantization);
bt_quantize_clamp(m_node_array[nodeindex].m_quantizedAabbMin,
bound.m_min,
m_global_bound.m_min,
m_global_bound.m_max,
m_bvhQuantization);
bt_quantize_clamp( m_node_array[nodeindex].m_quantizedAabbMax,
bound.m_max,
m_global_bound.m_min,
m_global_bound.m_max,
m_bvhQuantization);
bt_quantize_clamp(m_node_array[nodeindex].m_quantizedAabbMax,
bound.m_max,
m_global_bound.m_min,
m_global_bound.m_max,
m_bvhQuantization);
}
SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
{
return nodeindex+1;
return nodeindex + 1;
}
SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
{
if(m_node_array[nodeindex+1].isLeafNode()) return nodeindex+2;
return nodeindex+1 + m_node_array[nodeindex+1].getEscapeIndex();
if (m_node_array[nodeindex + 1].isLeafNode()) return nodeindex + 2;
return nodeindex + 1 + m_node_array[nodeindex + 1].getEscapeIndex();
}
SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
@@ -142,7 +140,7 @@ public:
return m_node_array[nodeindex].getEscapeIndex();
}
SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE* get_node_pointer(int index = 0) const
{
return &m_node_array[index];
}
@@ -150,8 +148,6 @@ public:
//!@}
};
//! Structure for containing Boxes
/*!
This class offers an structure for managing a box tree of primitives.
@@ -161,13 +157,13 @@ class btGImpactQuantizedBvh
{
protected:
btQuantizedBvhTree m_box_tree;
btPrimitiveManagerBase * m_primitive_manager;
btPrimitiveManagerBase* m_primitive_manager;
protected:
//stackless refit
void refit();
public:
public:
//! this constructor doesn't build the tree. you must call buildSet
btGImpactQuantizedBvh()
{
@@ -175,31 +171,30 @@ public:
}
//! this constructor doesn't build the tree. you must call buildSet
btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager)
btGImpactQuantizedBvh(btPrimitiveManagerBase* primitive_manager)
{
m_primitive_manager = primitive_manager;
}
SIMD_FORCE_INLINE btAABB getGlobalBox() const
SIMD_FORCE_INLINE btAABB getGlobalBox() const
{
btAABB totalbox;
getNodeBound(0, totalbox);
return totalbox;
}
SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager)
SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase* primitive_manager)
{
m_primitive_manager = primitive_manager;
}
SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const
SIMD_FORCE_INLINE btPrimitiveManagerBase* getPrimitiveManager() const
{
return m_primitive_manager;
}
//! node manager prototype functions
///@{
//! node manager prototype functions
///@{
//! this attemps to refit the box set.
SIMD_FORCE_INLINE void update()
@@ -211,21 +206,21 @@ public:
void buildSet();
//! returns the indices of the primitives in the m_primitive_manager
bool boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const;
bool boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const;
//! returns the indices of the primitives in the m_primitive_manager
SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB & box,
const btTransform & transform, btAlignedObjectArray<int> & collided_results) const
SIMD_FORCE_INLINE bool boxQueryTrans(const btAABB& box,
const btTransform& transform, btAlignedObjectArray<int>& collided_results) const
{
btAABB transbox=box;
btAABB transbox = box;
transbox.appy_transform(transform);
return boxQuery(transbox,collided_results);
return boxQuery(transbox, collided_results);
}
//! returns the indices of the primitives in the m_primitive_manager
bool rayQuery(
const btVector3 & ray_dir,const btVector3 & ray_origin ,
btAlignedObjectArray<int> & collided_results) const;
const btVector3& ray_dir, const btVector3& ray_origin,
btAlignedObjectArray<int>& collided_results) const;
//! tells if this set has hierarcht
SIMD_FORCE_INLINE bool hasHierarchy() const
@@ -234,7 +229,7 @@ public:
}
//! tells if this set is a trimesh
SIMD_FORCE_INLINE bool isTrimesh() const
SIMD_FORCE_INLINE bool isTrimesh() const
{
return m_primitive_manager->is_trimesh();
}
@@ -256,17 +251,16 @@ public:
return m_box_tree.getNodeData(nodeindex);
}
SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB & bound) const
SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB& bound) const
{
m_box_tree.getNodeBound(nodeindex, bound);
}
SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB & bound)
SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB& bound)
{
m_box_tree.setNodeBound(nodeindex, bound);
}
SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
{
return m_box_tree.getLeftNode(nodeindex);
@@ -282,24 +276,23 @@ public:
return m_box_tree.getEscapeNodeIndex(nodeindex);
}
SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const
SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex, btPrimitiveTriangle& triangle) const
{
m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle);
m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex), triangle);
}
SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const
SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE* get_node_pointer(int index = 0) const
{
return m_box_tree.get_node_pointer(index);
}
#ifdef TRI_COLLISION_PROFILING
static float getAverageTreeCollisionTime();
#endif //TRI_COLLISION_PROFILING
#endif //TRI_COLLISION_PROFILING
static void find_collision(const btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
const btGImpactQuantizedBvh * boxset2, const btTransform & trans2,
btPairSet & collision_pairs);
static void find_collision(const btGImpactQuantizedBvh* boxset1, const btTransform& trans1,
const btGImpactQuantizedBvh* boxset2, const btTransform& trans2,
btPairSet& collision_pairs);
};
#endif // GIM_BOXPRUNING_H_INCLUDED
#endif // GIM_BOXPRUNING_H_INCLUDED

28
src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h
View File

@@ -29,13 +29,14 @@ subject to the following restrictions:
///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE
ATTRIBUTE_ALIGNED16(struct)
BT_QUANTIZED_BVH_NODE
{
//12 bytes
unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3];
unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3];
//4 bytes
int m_escapeIndexOrDataIndex;
int m_escapeIndexOrDataIndex;
BT_QUANTIZED_BVH_NODE()
{
@@ -45,7 +46,7 @@ ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE
SIMD_FORCE_INLINE bool isLeafNode() const
{
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
return (m_escapeIndexOrDataIndex>=0);
return (m_escapeIndexOrDataIndex >= 0);
}
SIMD_FORCE_INLINE int getEscapeIndex() const
@@ -72,20 +73,19 @@ ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE
}
SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(
unsigned short * quantizedMin,unsigned short * quantizedMax) const
unsigned short* quantizedMin, unsigned short* quantizedMax) const
{
if(m_quantizedAabbMin[0] > quantizedMax[0] ||
m_quantizedAabbMax[0] < quantizedMin[0] ||
m_quantizedAabbMin[1] > quantizedMax[1] ||
m_quantizedAabbMax[1] < quantizedMin[1] ||
m_quantizedAabbMin[2] > quantizedMax[2] ||
m_quantizedAabbMax[2] < quantizedMin[2])
if (m_quantizedAabbMin[0] > quantizedMax[0] ||
m_quantizedAabbMax[0] < quantizedMin[0] ||
m_quantizedAabbMin[1] > quantizedMax[1] ||
m_quantizedAabbMax[1] < quantizedMin[1] ||
m_quantizedAabbMin[2] > quantizedMax[2] ||
m_quantizedAabbMax[2] < quantizedMin[2])
{
return false;
}
return true;
}
};
#endif // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED
#endif // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED

198
src/BulletCollision/Gimpact/btGImpactShape.cpp
View File

@@ -18,178 +18,169 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btGImpactShape.h"
#include "btGImpactMassUtil.h"
btGImpactMeshShapePart::btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part )
btGImpactMeshShapePart::btGImpactMeshShapePart(btStridingMeshInterface* meshInterface, int part)
{
// moved from .h to .cpp because of conditional compilation
// (The setting of BT_THREADSAFE may differ between various cpp files, so it is best to
// avoid using it in h files)
m_primitive_manager.m_meshInterface = meshInterface;
m_primitive_manager.m_part = part;
m_box_set.setPrimitiveManager( &m_primitive_manager );
// moved from .h to .cpp because of conditional compilation
// (The setting of BT_THREADSAFE may differ between various cpp files, so it is best to
// avoid using it in h files)
m_primitive_manager.m_meshInterface = meshInterface;
m_primitive_manager.m_part = part;
m_box_set.setPrimitiveManager(&m_primitive_manager);
#if BT_THREADSAFE
// If threadsafe is requested, this object uses a different lock/unlock
// model with the btStridingMeshInterface -- lock once when the object is constructed
// and unlock once in the destructor.
// The other way of locking and unlocking for each collision check in the narrowphase
// is not threadsafe. Note these are not thread-locks, they are calls to the meshInterface's
// getLockedReadOnlyVertexIndexBase virtual function, which by default just returns a couple of
// pointers. In theory a client could override the lock function to do all sorts of
// things like reading data from GPU memory, or decompressing data on the fly, but such things
// do not seem all that likely or useful, given the performance cost.
m_primitive_manager.lock();
// If threadsafe is requested, this object uses a different lock/unlock
// model with the btStridingMeshInterface -- lock once when the object is constructed
// and unlock once in the destructor.
// The other way of locking and unlocking for each collision check in the narrowphase
// is not threadsafe. Note these are not thread-locks, they are calls to the meshInterface's
// getLockedReadOnlyVertexIndexBase virtual function, which by default just returns a couple of
// pointers. In theory a client could override the lock function to do all sorts of
// things like reading data from GPU memory, or decompressing data on the fly, but such things
// do not seem all that likely or useful, given the performance cost.
m_primitive_manager.lock();
#endif
}
btGImpactMeshShapePart::~btGImpactMeshShapePart()
{
// moved from .h to .cpp because of conditional compilation
// moved from .h to .cpp because of conditional compilation
#if BT_THREADSAFE
m_primitive_manager.unlock();
m_primitive_manager.unlock();
#endif
}
void btGImpactMeshShapePart::lockChildShapes() const
{
// moved from .h to .cpp because of conditional compilation
#if ! BT_THREADSAFE
// called in the narrowphase -- not threadsafe!
void * dummy = (void*) ( m_box_set.getPrimitiveManager() );
TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>( dummy );
dummymanager->lock();
// moved from .h to .cpp because of conditional compilation
#if !BT_THREADSAFE
// called in the narrowphase -- not threadsafe!
void* dummy = (void*)(m_box_set.getPrimitiveManager());
TrimeshPrimitiveManager* dummymanager = static_cast<TrimeshPrimitiveManager*>(dummy);
dummymanager->lock();
#endif
}
void btGImpactMeshShapePart::unlockChildShapes() const
void btGImpactMeshShapePart::unlockChildShapes() const
{
// moved from .h to .cpp because of conditional compilation
#if ! BT_THREADSAFE
// called in the narrowphase -- not threadsafe!
void * dummy = (void*) ( m_box_set.getPrimitiveManager() );
TrimeshPrimitiveManager * dummymanager = static_cast<TrimeshPrimitiveManager *>( dummy );
dummymanager->unlock();
// moved from .h to .cpp because of conditional compilation
#if !BT_THREADSAFE
// called in the narrowphase -- not threadsafe!
void* dummy = (void*)(m_box_set.getPrimitiveManager());
TrimeshPrimitiveManager* dummymanager = static_cast<TrimeshPrimitiveManager*>(dummy);
dummymanager->unlock();
#endif
}
#define CALC_EXACT_INERTIA 1
void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
void btGImpactCompoundShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
{
lockChildShapes();
#ifdef CALC_EXACT_INERTIA
inertia.setValue(0.f,0.f,0.f);
inertia.setValue(0.f, 0.f, 0.f);
int i = this->getNumChildShapes();
btScalar shapemass = mass/btScalar(i);
btScalar shapemass = mass / btScalar(i);
while(i--)
while (i--)
{
btVector3 temp_inertia;
m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia);
if(childrenHasTransform())
m_childShapes[i]->calculateLocalInertia(shapemass, temp_inertia);
if (childrenHasTransform())
{
inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]);
inertia = gim_inertia_add_transformed(inertia, temp_inertia, m_childTransforms[i]);
}
else
{
inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity());
inertia = gim_inertia_add_transformed(inertia, temp_inertia, btTransform::getIdentity());
}
}
#else
// Calc box inertia
btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
const btScalar x2 = lx*lx;
const btScalar y2 = ly*ly;
const btScalar z2 = lz*lz;
btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0];
btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1];
btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2];
const btScalar x2 = lx * lx;
const btScalar y2 = ly * ly;
const btScalar z2 = lz * lz;
const btScalar scaledmass = mass * btScalar(0.08333333);
inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
#endif
unlockChildShapes();
}
void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const
void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass, btVector3& inertia) const
{
lockChildShapes();
#ifdef CALC_EXACT_INERTIA
inertia.setValue(0.f,0.f,0.f);
inertia.setValue(0.f, 0.f, 0.f);
int i = this->getVertexCount();
btScalar pointmass = mass/btScalar(i);
btScalar pointmass = mass / btScalar(i);
while(i--)
while (i--)
{
btVector3 pointintertia;
this->getVertex(i,pointintertia);
pointintertia = gim_get_point_inertia(pointintertia,pointmass);
inertia+=pointintertia;
this->getVertex(i, pointintertia);
pointintertia = gim_get_point_inertia(pointintertia, pointmass);
inertia += pointintertia;
}
#else
// Calc box inertia
btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
const btScalar x2 = lx*lx;
const btScalar y2 = ly*ly;
const btScalar z2 = lz*lz;
btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0];
btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1];
btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2];
const btScalar x2 = lx * lx;
const btScalar y2 = ly * ly;
const btScalar z2 = lz * lz;
const btScalar scaledmass = mass * btScalar(0.08333333);
inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
#endif
unlockChildShapes();
}
void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
void btGImpactMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
{
#ifdef CALC_EXACT_INERTIA
inertia.setValue(0.f,0.f,0.f);
inertia.setValue(0.f, 0.f, 0.f);
int i = this->getMeshPartCount();
btScalar partmass = mass/btScalar(i);
btScalar partmass = mass / btScalar(i);
while(i--)
while (i--)
{
btVector3 partinertia;
getMeshPart(i)->calculateLocalInertia(partmass,partinertia);
inertia+=partinertia;
getMeshPart(i)->calculateLocalInertia(partmass, partinertia);
inertia += partinertia;
}
#else
// Calc box inertia
btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
const btScalar x2 = lx*lx;
const btScalar y2 = ly*ly;
const btScalar z2 = lz*lz;
btScalar lx = m_localAABB.m_max[0] - m_localAABB.m_min[0];
btScalar ly = m_localAABB.m_max[1] - m_localAABB.m_min[1];
btScalar lz = m_localAABB.m_max[2] - m_localAABB.m_min[2];
const btScalar x2 = lx * lx;
const btScalar y2 = ly * ly;
const btScalar z2 = lz * lz;
const btScalar scaledmass = mass * btScalar(0.08333333);
inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));
inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2));
#endif
}
@@ -198,7 +189,7 @@ void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayT
{
}
void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const
void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const
{
lockChildShapes();
@@ -207,7 +198,7 @@ void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback
rayDir.normalize();
m_box_set.rayQuery(rayDir, rayFrom, collided);
if(collided.size()==0)
if (collided.size() == 0)
{
unlockChildShapes();
return;
@@ -216,15 +207,15 @@ void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback
int part = (int)getPart();
btPrimitiveTriangle triangle;
int i = collided.size();
while(i--)
while (i--)
{
getPrimitiveTriangle(collided[i],triangle);
callback->processTriangle(triangle.m_vertices,part,collided[i]);
getPrimitiveTriangle(collided[i], triangle);
callback->processTriangle(triangle.m_vertices, part, collided[i]);
}
unlockChildShapes();
}
void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
{
lockChildShapes();
btAABB box;
@@ -232,9 +223,9 @@ void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,co
box.m_max = aabbMax;
btAlignedObjectArray<int> collided;
m_box_set.boxQuery(box,collided);
m_box_set.boxQuery(box, collided);
if(collided.size()==0)
if (collided.size() == 0)
{
unlockChildShapes();
return;
@@ -243,40 +234,38 @@ void btGImpactMeshShapePart::processAllTriangles(btTriangleCallback* callback,co
int part = (int)getPart();
btPrimitiveTriangle triangle;
int i = collided.size();
while(i--)
while (i--)
{
this->getPrimitiveTriangle(collided[i],triangle);
callback->processTriangle(triangle.m_vertices,part,collided[i]);
this->getPrimitiveTriangle(collided[i], triangle);
callback->processTriangle(triangle.m_vertices, part, collided[i]);
}
unlockChildShapes();
}
void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const
{
int i = m_mesh_parts.size();
while(i--)
while (i--)
{
m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax);
m_mesh_parts[i]->processAllTriangles(callback, aabbMin, aabbMax);
}
}
void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const
void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback, const btVector3& rayFrom, const btVector3& rayTo) const
{
int i = m_mesh_parts.size();
while(i--)
while (i--)
{
m_mesh_parts[i]->processAllTrianglesRay(callback, rayFrom, rayTo);
}
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const
{
btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*) dataBuffer;
btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*)dataBuffer;
btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer);
btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer);
m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer);
@@ -288,4 +277,3 @@ const char* btGImpactMeshShape::serialize(void* dataBuffer, btSerializer* serial
return "btGImpactMeshShapeData";
}

655
src/BulletCollision/Gimpact/btGImpactShape.h
View File

File diff suppressed because it is too large Load Diff

125
src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp
View File

@@ -20,48 +20,45 @@ subject to the following restrictions:
#include "btGenericPoolAllocator.h"
/// *************** btGenericMemoryPool ******************///////////
size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements)
{
size_t ptr = BT_UINT_MAX;
if(m_free_nodes_count == 0) return BT_UINT_MAX;
if (m_free_nodes_count == 0) return BT_UINT_MAX;
// find an avaliable free node with the correct size
size_t revindex = m_free_nodes_count;
while(revindex-- && ptr == BT_UINT_MAX)
while (revindex-- && ptr == BT_UINT_MAX)
{
if(m_allocated_sizes[m_free_nodes[revindex]]>=num_elements)
if (m_allocated_sizes[m_free_nodes[revindex]] >= num_elements)
{
ptr = revindex;
}
}
if(ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found
if (ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found
revindex = ptr;
ptr = m_free_nodes[revindex];
// post: ptr contains the node index, and revindex the index in m_free_nodes
size_t finalsize = m_allocated_sizes[ptr];
size_t finalsize = m_allocated_sizes[ptr];
finalsize -= num_elements;
m_allocated_sizes[ptr] = num_elements;
// post: finalsize>=0, m_allocated_sizes[ptr] has the requested size
if(finalsize>0) // preserve free node, there are some free memory
if (finalsize > 0) // preserve free node, there are some free memory
{
m_free_nodes[revindex] = ptr + num_elements;
m_allocated_sizes[ptr + num_elements] = finalsize;
}
else // delete free node
else // delete free node
{
// swap with end
m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count-1];
m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count - 1];
m_free_nodes_count--;
}
@@ -70,17 +67,16 @@ size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements)
size_t btGenericMemoryPool::allocate_from_pool(size_t num_elements)
{
if(m_allocated_count+num_elements>m_max_element_count) return BT_UINT_MAX;
if (m_allocated_count + num_elements > m_max_element_count) return BT_UINT_MAX;
size_t ptr = m_allocated_count;
m_allocated_sizes[m_allocated_count] = num_elements;
m_allocated_count+=num_elements;
m_allocated_count += num_elements;
return ptr;
}
void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count)
{
m_allocated_count = 0;
@@ -89,14 +85,11 @@ void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count)
m_element_size = element_size;
m_max_element_count = element_count;
m_pool = (unsigned char *)btAlignedAlloc(m_element_size * m_max_element_count, 16);
m_free_nodes = (size_t *)btAlignedAlloc(sizeof(size_t) * m_max_element_count, 16);
m_allocated_sizes = (size_t *)btAlignedAlloc(sizeof(size_t) * m_max_element_count, 16);
m_pool = (unsigned char *) btAlignedAlloc(m_element_size*m_max_element_count,16);
m_free_nodes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16);
m_allocated_sizes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16);
for (size_t i = 0;i< m_max_element_count;i++ )
for (size_t i = 0; i < m_max_element_count; i++)
{
m_allocated_sizes[i] = 0;
}
@@ -111,150 +104,141 @@ void btGenericMemoryPool::end_pool()
m_free_nodes_count = 0;
}
//! Allocates memory in pool
/*!
\param size_bytes size in bytes of the buffer
*/
void * btGenericMemoryPool::allocate(size_t size_bytes)
void *btGenericMemoryPool::allocate(size_t size_bytes)
{
size_t module = size_bytes%m_element_size;
size_t element_count = size_bytes/m_element_size;
if(module>0) element_count++;
size_t module = size_bytes % m_element_size;
size_t element_count = size_bytes / m_element_size;
if (module > 0) element_count++;
size_t alloc_pos = allocate_from_free_nodes(element_count);
// a free node is found
if(alloc_pos != BT_UINT_MAX)
if (alloc_pos != BT_UINT_MAX)
{
return get_element_data(alloc_pos);
}
// allocate directly on pool
alloc_pos = allocate_from_pool(element_count);
if(alloc_pos == BT_UINT_MAX) return NULL; // not space
if (alloc_pos == BT_UINT_MAX) return NULL; // not space
return get_element_data(alloc_pos);
}
bool btGenericMemoryPool::freeMemory(void * pointer)
bool btGenericMemoryPool::freeMemory(void *pointer)
{
unsigned char * pointer_pos = (unsigned char *)pointer;
unsigned char * pool_pos = (unsigned char *)m_pool;
unsigned char *pointer_pos = (unsigned char *)pointer;
unsigned char *pool_pos = (unsigned char *)m_pool;
// calc offset
if(pointer_pos<pool_pos) return false;//other pool
if (pointer_pos < pool_pos) return false; //other pool
size_t offset = size_t(pointer_pos - pool_pos);
if(offset>=get_pool_capacity()) return false;// far away
if (offset >= get_pool_capacity()) return false; // far away
// find free position
m_free_nodes[m_free_nodes_count] = offset/m_element_size;
m_free_nodes[m_free_nodes_count] = offset / m_element_size;
m_free_nodes_count++;
return true;
}
/// *******************! btGenericPoolAllocator *******************!///
btGenericPoolAllocator::~btGenericPoolAllocator()
{
// destroy pools
size_t i;
for (i=0;i<m_pool_count;i++)
for (i = 0; i < m_pool_count; i++)
{
m_pools[i]->end_pool();
btAlignedFree(m_pools[i]);
}
}
// creates a pool
btGenericMemoryPool * btGenericPoolAllocator::push_new_pool()
btGenericMemoryPool *btGenericPoolAllocator::push_new_pool()
{
if(m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL;
if (m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL;
btGenericMemoryPool * newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool),16);
btGenericMemoryPool *newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool), 16);
m_pools[m_pool_count] = newptr;
m_pools[m_pool_count]->init_pool(m_pool_element_size,m_pool_element_count);
m_pools[m_pool_count]->init_pool(m_pool_element_size, m_pool_element_count);
m_pool_count++;
return newptr;
}
void * btGenericPoolAllocator::failback_alloc(size_t size_bytes)
void *btGenericPoolAllocator::failback_alloc(size_t size_bytes)
{
btGenericMemoryPool *pool = NULL;
btGenericMemoryPool * pool = NULL;
if(size_bytes<=get_pool_capacity())
if (size_bytes <= get_pool_capacity())
{
pool = push_new_pool();
pool = push_new_pool();
}
if(pool==NULL) // failback
if (pool == NULL) // failback
{
return btAlignedAlloc(size_bytes,16);
return btAlignedAlloc(size_bytes, 16);
}
return pool->allocate(size_bytes);
}
bool btGenericPoolAllocator::failback_free(void * pointer)
bool btGenericPoolAllocator::failback_free(void *pointer)
{
btAlignedFree(pointer);
return true;
}
//! Allocates memory in pool
/*!
\param size_bytes size in bytes of the buffer
*/
void * btGenericPoolAllocator::allocate(size_t size_bytes)
void *btGenericPoolAllocator::allocate(size_t size_bytes)
{
void * ptr = NULL;
void *ptr = NULL;
size_t i = 0;
while(i<m_pool_count && ptr == NULL)
while (i < m_pool_count && ptr == NULL)
{
ptr = m_pools[i]->allocate(size_bytes);
++i;
}
if(ptr) return ptr;
if (ptr) return ptr;
return failback_alloc(size_bytes);
}
bool btGenericPoolAllocator::freeMemory(void * pointer)
bool btGenericPoolAllocator::freeMemory(void *pointer)
{
bool result = false;
size_t i = 0;
while(i<m_pool_count && result == false)
while (i < m_pool_count && result == false)
{
result = m_pools[i]->freeMemory(pointer);
++i;
}
if(result) return true;
if (result) return true;
return failback_free(pointer);
}
/// ************** STANDARD ALLOCATOR ***************************///
#define BT_DEFAULT_POOL_SIZE 32768
#define BT_DEFAULT_POOL_ELEMENT_SIZE 8
// main allocator
class GIM_STANDARD_ALLOCATOR: public btGenericPoolAllocator
class GIM_STANDARD_ALLOCATOR : public btGenericPoolAllocator
{
public:
GIM_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE)
GIM_STANDARD_ALLOCATOR() : btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE, BT_DEFAULT_POOL_SIZE)
{
}
};
@@ -262,19 +246,18 @@ public:
// global allocator
GIM_STANDARD_ALLOCATOR g_main_allocator;
void * btPoolAlloc(size_t size)
void *btPoolAlloc(size_t size)
{
return g_main_allocator.allocate(size);
}
void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize)
void *btPoolRealloc(void *ptr, size_t oldsize, size_t newsize)
{
void * newptr = btPoolAlloc(newsize);
size_t copysize = oldsize<newsize?oldsize:newsize;
memcpy(newptr,ptr,copysize);
btPoolFree(ptr);
return newptr;
void *newptr = btPoolAlloc(newsize);
size_t copysize = oldsize < newsize ? oldsize : newsize;
memcpy(newptr, ptr, copysize);
btPoolFree(ptr);
return newptr;
}
void btPoolFree(void *ptr)

52
src/BulletCollision/Gimpact/btGenericPoolAllocator.h
View File

@@ -29,16 +29,16 @@ subject to the following restrictions:
#define BT_UINT_MAX UINT_MAX
#define BT_DEFAULT_MAX_POOLS 16
//! Generic Pool class
class btGenericMemoryPool
{
public:
unsigned char * m_pool; //[m_element_size*m_max_element_count];
size_t * m_free_nodes; //[m_max_element_count];//! free nodes
size_t * m_allocated_sizes;//[m_max_element_count];//! Number of elements allocated per node
unsigned char *m_pool; //[m_element_size*m_max_element_count];
size_t *m_free_nodes; //[m_max_element_count];//! free nodes
size_t *m_allocated_sizes; //[m_max_element_count];//! Number of elements allocated per node
size_t m_allocated_count;
size_t m_free_nodes_count;
protected:
size_t m_element_size;
size_t m_max_element_count;
@@ -47,12 +47,10 @@ protected:
size_t allocate_from_pool(size_t num_elements);
public:
void init_pool(size_t element_size, size_t element_count);
void end_pool();
btGenericMemoryPool(size_t element_size, size_t element_count)
{
init_pool(element_size, element_count);
@@ -63,10 +61,9 @@ public:
end_pool();
}
inline size_t get_pool_capacity()
{
return m_element_size*m_max_element_count;
return m_element_size * m_max_element_count;
}
inline size_t gem_element_size()
@@ -89,23 +86,20 @@ public:
return m_free_nodes_count;
}
inline void * get_element_data(size_t element_index)
inline void *get_element_data(size_t element_index)
{
return &m_pool[element_index*m_element_size];
return &m_pool[element_index * m_element_size];
}
//! Allocates memory in pool
/*!
\param size_bytes size in bytes of the buffer
*/
void * allocate(size_t size_bytes);
void *allocate(size_t size_bytes);
bool freeMemory(void * pointer);
bool freeMemory(void *pointer);
};
//! Generic Allocator with pools
/*!
General purpose Allocator which can create Memory Pools dynamiacally as needed.
@@ -115,26 +109,25 @@ class btGenericPoolAllocator
protected:
size_t m_pool_element_size;
size_t m_pool_element_count;
public:
btGenericMemoryPool * m_pools[BT_DEFAULT_MAX_POOLS];
size_t m_pool_count;
public:
btGenericMemoryPool *m_pools[BT_DEFAULT_MAX_POOLS];
size_t m_pool_count;
inline size_t get_pool_capacity()
{
return m_pool_element_size*m_pool_element_count;
return m_pool_element_size * m_pool_element_count;
}
protected:
// creates a pool
btGenericMemoryPool * push_new_pool();
btGenericMemoryPool *push_new_pool();
void * failback_alloc(size_t size_bytes);
void *failback_alloc(size_t size_bytes);
bool failback_free(void *pointer);
bool failback_free(void * pointer);
public:
btGenericPoolAllocator(size_t pool_element_size, size_t pool_element_count)
{
m_pool_count = 0;
@@ -148,16 +141,13 @@ public:
/*!
\param size_bytes size in bytes of the buffer
*/
void * allocate(size_t size_bytes);
void *allocate(size_t size_bytes);
bool freeMemory(void * pointer);
bool freeMemory(void *pointer);
};
void * btPoolAlloc(size_t size);
void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize);
void *btPoolAlloc(size_t size);
void *btPoolRealloc(void *ptr, size_t oldsize, size_t newsize);
void btPoolFree(void *ptr);
#endif

214
src/BulletCollision/Gimpact/btGeometryOperations.h
View File

@@ -27,52 +27,44 @@ subject to the following restrictions:
#include "btBoxCollision.h"
#define PLANEDIREPSILON 0.0000001f
#define PARALELENORMALS 0.000001f
#define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
#define BT_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number))
/// Calc a plane from a triangle edge an a normal. plane is a vec4f
SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 & e2, const btVector3 & normal,btVector4 & plane)
SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 &e1, const btVector3 &e2, const btVector3 &normal, btVector4 &plane)
{
btVector3 planenormal = (e2-e1).cross(normal);
btVector3 planenormal = (e2 - e1).cross(normal);
planenormal.normalize();
plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal));
plane.setValue(planenormal[0], planenormal[1], planenormal[2], e2.dot(planenormal));
}
//***************** SEGMENT and LINE FUNCTIONS **********************************///
/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
*/
SIMD_FORCE_INLINE void bt_closest_point_on_segment(
btVector3 & cp, const btVector3 & v,
const btVector3 &e1,const btVector3 &e2)
btVector3 &cp, const btVector3 &v,
const btVector3 &e1, const btVector3 &e2)
{
btVector3 n = e2-e1;
cp = v - e1;
btScalar _scalar = cp.dot(n)/n.dot(n);
if(_scalar <0.0f)
btVector3 n = e2 - e1;
cp = v - e1;
btScalar _scalar = cp.dot(n) / n.dot(n);
if (_scalar < 0.0f)
{
cp = e1;
cp = e1;
}
else if(_scalar >1.0f)
else if (_scalar > 1.0f)
{
cp = e2;
cp = e2;
}
else
{
cp = _scalar*n + e1;
cp = _scalar * n + e1;
}
}
//! line plane collision
/*!
*\return
@@ -82,131 +74,125 @@ SIMD_FORCE_INLINE void bt_closest_point_on_segment(
*/
SIMD_FORCE_INLINE int bt_line_plane_collision(
const btVector4 & plane,
const btVector3 & vDir,
const btVector3 & vPoint,
btVector3 & pout,
const btVector4 &plane,
const btVector3 &vDir,
const btVector3 &vPoint,
btVector3 &pout,
btScalar &tparam,
btScalar tmin, btScalar tmax)
{
btScalar _dotdir = vDir.dot(plane);
if(btFabs(_dotdir)<PLANEDIREPSILON)
if (btFabs(_dotdir) < PLANEDIREPSILON)
{
tparam = tmax;
return 0;
return 0;
}
btScalar _dis = bt_distance_point_plane(plane,vPoint);
char returnvalue = _dis<0.0f? 2:1;
tparam = -_dis/_dotdir;
btScalar _dis = bt_distance_point_plane(plane, vPoint);
char returnvalue = _dis < 0.0f ? 2 : 1;
tparam = -_dis / _dotdir;
if(tparam<tmin)
if (tparam < tmin)
{
returnvalue = 0;
tparam = tmin;
}
else if(tparam>tmax)
else if (tparam > tmax)
{
returnvalue = 0;
tparam = tmax;
}
pout = tparam*vDir + vPoint;
pout = tparam * vDir + vPoint;
return returnvalue;
}
//! Find closest points on segments
SIMD_FORCE_INLINE void bt_segment_collision(
const btVector3 & vA1,
const btVector3 & vA2,
const btVector3 & vB1,
const btVector3 & vB2,
btVector3 & vPointA,
btVector3 & vPointB)
const btVector3 &vA1,
const btVector3 &vA2,
const btVector3 &vB1,
const btVector3 &vB2,
btVector3 &vPointA,
btVector3 &vPointB)
{
btVector3 AD = vA2 - vA1;
btVector3 BD = vB2 - vB1;
btVector3 N = AD.cross(BD);
btScalar tp = N.length2();
btVector3 AD = vA2 - vA1;
btVector3 BD = vB2 - vB1;
btVector3 N = AD.cross(BD);
btScalar tp = N.length2();
btVector4 _M;//plane
btVector4 _M; //plane
if(tp<SIMD_EPSILON)//ARE PARALELE
{
//project B over A
bool invert_b_order = false;
_M[0] = vB1.dot(AD);
_M[1] = vB2.dot(AD);
if (tp < SIMD_EPSILON) //ARE PARALELE
{
//project B over A
bool invert_b_order = false;
_M[0] = vB1.dot(AD);
_M[1] = vB2.dot(AD);
if(_M[0]>_M[1])
{
invert_b_order = true;
BT_SWAP_NUMBERS(_M[0],_M[1]);
}
_M[2] = vA1.dot(AD);
_M[3] = vA2.dot(AD);
//mid points
N[0] = (_M[0]+_M[1])*0.5f;
N[1] = (_M[2]+_M[3])*0.5f;
if (_M[0] > _M[1])
{
invert_b_order = true;
BT_SWAP_NUMBERS(_M[0], _M[1]);
}
_M[2] = vA1.dot(AD);
_M[3] = vA2.dot(AD);
//mid points
N[0] = (_M[0] + _M[1]) * 0.5f;
N[1] = (_M[2] + _M[3]) * 0.5f;
if(N[0]<N[1])
{
if(_M[1]<_M[2])
{
vPointB = invert_b_order?vB1:vB2;
vPointA = vA1;
}
else if(_M[1]<_M[3])
{
vPointB = invert_b_order?vB1:vB2;
bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
}
else
{
vPointA = vA2;
bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
}
}
else
{
if(_M[3]<_M[0])
{
vPointB = invert_b_order?vB2:vB1;
vPointA = vA2;
}
else if(_M[3]<_M[1])
{
vPointA = vA2;
bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
}
else
{
vPointB = invert_b_order?vB1:vB2;
bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
}
}
return;
}
if (N[0] < N[1])
{
if (_M[1] < _M[2])
{
vPointB = invert_b_order ? vB1 : vB2;
vPointA = vA1;
}
else if (_M[1] < _M[3])
{
vPointB = invert_b_order ? vB1 : vB2;
bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2);
}
else
{
vPointA = vA2;
bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2);
}
}
else
{
if (_M[3] < _M[0])
{
vPointB = invert_b_order ? vB2 : vB1;
vPointA = vA2;
}
else if (_M[3] < _M[1])
{
vPointA = vA2;
bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2);
}
else
{
vPointB = invert_b_order ? vB1 : vB2;
bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2);
}
}
return;
}
N = N.cross(BD);
_M.setValue(N[0],N[1],N[2],vB1.dot(N));
N = N.cross(BD);
_M.setValue(N[0], N[1], N[2], vB1.dot(N));
// get point A as the plane collision point
bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1));
bt_line_plane_collision(_M, AD, vA1, vPointA, tp, btScalar(0), btScalar(1));
/*Closest point on segment*/
vPointB = vPointA - vB1;
/*Closest point on segment*/
vPointB = vPointA - vB1;
tp = vPointB.dot(BD);
tp/= BD.dot(BD);
tp = BT_CLAMP(tp,0.0f,1.0f);
tp /= BD.dot(BD);
tp = BT_CLAMP(tp, 0.0f, 1.0f);
vPointB = tp*BD + vB1;
vPointB = tp * BD + vB1;
}
#endif // GIM_VECTOR_H_INCLUDED
#endif // GIM_VECTOR_H_INCLUDED

43
src/BulletCollision/Gimpact/btQuantization.h
View File

@@ -27,54 +27,47 @@ subject to the following restrictions:
#include "LinearMath/btTransform.h"
SIMD_FORCE_INLINE void bt_calc_quantization_parameters(
btVector3 & outMinBound,
btVector3 & outMaxBound,
btVector3 & bvhQuantization,
const btVector3& srcMinBound,const btVector3& srcMaxBound,
btVector3& outMinBound,
btVector3& outMaxBound,
btVector3& bvhQuantization,
const btVector3& srcMinBound, const btVector3& srcMaxBound,
btScalar quantizationMargin)
{
//enlarge the AABB to avoid division by zero when initializing the quantization values
btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin);
btVector3 clampValue(quantizationMargin, quantizationMargin, quantizationMargin);
outMinBound = srcMinBound - clampValue;
outMaxBound = srcMaxBound + clampValue;
btVector3 aabbSize = outMaxBound - outMinBound;
bvhQuantization = btVector3(btScalar(65535.0),
btScalar(65535.0),
btScalar(65535.0)) / aabbSize;
btScalar(65535.0)) /
aabbSize;
}
SIMD_FORCE_INLINE void bt_quantize_clamp(
unsigned short* out,
const btVector3& point,
const btVector3 & min_bound,
const btVector3 & max_bound,
const btVector3 & bvhQuantization)
const btVector3& min_bound,
const btVector3& max_bound,
const btVector3& bvhQuantization)
{
btVector3 clampedPoint(point);
clampedPoint.setMax(min_bound);
clampedPoint.setMin(max_bound);
btVector3 v = (clampedPoint - min_bound) * bvhQuantization;
out[0] = (unsigned short)(v.getX()+0.5f);
out[1] = (unsigned short)(v.getY()+0.5f);
out[2] = (unsigned short)(v.getZ()+0.5f);
out[0] = (unsigned short)(v.getX() + 0.5f);
out[1] = (unsigned short)(v.getY() + 0.5f);
out[2] = (unsigned short)(v.getZ() + 0.5f);
}
SIMD_FORCE_INLINE btVector3 bt_unquantize(
const unsigned short* vecIn,
const btVector3 & offset,
const btVector3 & bvhQuantization)
const btVector3& offset,
const btVector3& bvhQuantization)
{
btVector3 vecOut;
btVector3 vecOut;
vecOut.setValue(
(btScalar)(vecIn[0]) / (bvhQuantization.getX()),
(btScalar)(vecIn[1]) / (bvhQuantization.getY()),
@@ -83,6 +76,4 @@ SIMD_FORCE_INLINE btVector3 bt_unquantize(
return vecOut;
}
#endif // BT_GIMPACT_QUANTIZATION_H_INCLUDED
#endif // BT_GIMPACT_QUANTIZATION_H_INCLUDED

239
src/BulletCollision/Gimpact/btTriangleShapeEx.cpp
View File

@@ -23,196 +23,181 @@ subject to the following restrictions:
#include "btTriangleShapeEx.h"
void GIM_TRIANGLE_CONTACT::merge_points(const btVector4 & plane,
btScalar margin, const btVector3 * points, int point_count)
void GIM_TRIANGLE_CONTACT::merge_points(const btVector4& plane,
btScalar margin, const btVector3* points, int point_count)
{
m_point_count = 0;
m_penetration_depth= -1000.0f;
m_point_count = 0;
m_penetration_depth = -1000.0f;
int point_indices[MAX_TRI_CLIPPING];
int point_indices[MAX_TRI_CLIPPING];
int _k;
for ( _k=0;_k<point_count;_k++)
{
btScalar _dist = - bt_distance_point_plane(plane,points[_k]) + margin;
for (_k = 0; _k < point_count; _k++)
{
btScalar _dist = -bt_distance_point_plane(plane, points[_k]) + margin;
if (_dist>=0.0f)
{
if (_dist>m_penetration_depth)
{
m_penetration_depth = _dist;
point_indices[0] = _k;
m_point_count=1;
}
else if ((_dist+SIMD_EPSILON)>=m_penetration_depth)
{
point_indices[m_point_count] = _k;
m_point_count++;
}
}
}
if (_dist >= 0.0f)
{
if (_dist > m_penetration_depth)
{
m_penetration_depth = _dist;
point_indices[0] = _k;
m_point_count = 1;
}
else if ((_dist + SIMD_EPSILON) >= m_penetration_depth)
{
point_indices[m_point_count] = _k;
m_point_count++;
}
}
}
for ( _k=0;_k<m_point_count;_k++)
{
m_points[_k] = points[point_indices[_k]];
}
for (_k = 0; _k < m_point_count; _k++)
{
m_points[_k] = points[point_indices[_k]];
}
}
///class btPrimitiveTriangle
bool btPrimitiveTriangle::overlap_test_conservative(const btPrimitiveTriangle& other)
{
btScalar total_margin = m_margin + other.m_margin;
// classify points on other triangle
btScalar dis0 = bt_distance_point_plane(m_plane,other.m_vertices[0]) - total_margin;
btScalar total_margin = m_margin + other.m_margin;
// classify points on other triangle
btScalar dis0 = bt_distance_point_plane(m_plane, other.m_vertices[0]) - total_margin;
btScalar dis1 = bt_distance_point_plane(m_plane,other.m_vertices[1]) - total_margin;
btScalar dis1 = bt_distance_point_plane(m_plane, other.m_vertices[1]) - total_margin;
btScalar dis2 = bt_distance_point_plane(m_plane,other.m_vertices[2]) - total_margin;
btScalar dis2 = bt_distance_point_plane(m_plane, other.m_vertices[2]) - total_margin;
if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
// classify points on this triangle
dis0 = bt_distance_point_plane(other.m_plane,m_vertices[0]) - total_margin;
// classify points on this triangle
dis0 = bt_distance_point_plane(other.m_plane, m_vertices[0]) - total_margin;
dis1 = bt_distance_point_plane(other.m_plane,m_vertices[1]) - total_margin;
dis1 = bt_distance_point_plane(other.m_plane, m_vertices[1]) - total_margin;
dis2 = bt_distance_point_plane(other.m_plane,m_vertices[2]) - total_margin;
dis2 = bt_distance_point_plane(other.m_plane, m_vertices[2]) - total_margin;
if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
return true;
return true;
}
int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points )
int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle& other, btVector3* clipped_points)
{
// edge 0
// edge 0
btVector3 temp_points[MAX_TRI_CLIPPING];
btVector3 temp_points[MAX_TRI_CLIPPING];
btVector4 edgeplane;
btVector4 edgeplane;
get_edge_plane(0, 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;
int clipped_count = bt_plane_clip_triangle(
edgeplane,other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],temp_points);
btVector3 temp_points1[MAX_TRI_CLIPPING];
if (clipped_count == 0) return 0;
// edge 1
get_edge_plane(1, edgeplane);
btVector3 temp_points1[MAX_TRI_CLIPPING];
clipped_count = bt_plane_clip_polygon(edgeplane, temp_points, clipped_count, temp_points1);
if (clipped_count == 0) return 0;
// edge 1
get_edge_plane(1,edgeplane);
// edge 2
get_edge_plane(2, edgeplane);
clipped_count = bt_plane_clip_polygon(
edgeplane, temp_points1, clipped_count, clipped_points);
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;
return clipped_count;
}
bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts)
bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_TRIANGLE_CONTACT& contacts)
{
btScalar margin = m_margin + other.m_margin;
btScalar margin = m_margin + other.m_margin;
btVector3 clipped_points[MAX_TRI_CLIPPING];
int clipped_count;
//create planes
// plane v vs U points
btVector3 clipped_points[MAX_TRI_CLIPPING];
int clipped_count;
//create planes
// plane v vs U points
GIM_TRIANGLE_CONTACT contacts1;
GIM_TRIANGLE_CONTACT contacts1;
contacts1.m_separating_normal = m_plane;
contacts1.m_separating_normal = m_plane;
clipped_count = clip_triangle(other, clipped_points);
clipped_count = clip_triangle(other,clipped_points);
if (clipped_count == 0)
{
return false; //Reject
}
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;
//find most deep interval face1
contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
if (contacts1.m_point_count == 0) return false; // too far
//Normal pointing to this triangle
contacts1.m_separating_normal *= -1.f;
//Clip tri1 by tri2 edges
GIM_TRIANGLE_CONTACT contacts2;
contacts2.m_separating_normal = other.m_plane;
clipped_count = other.clip_triangle(*this, clipped_points);
//Clip tri1 by tri2 edges
GIM_TRIANGLE_CONTACT contacts2;
contacts2.m_separating_normal = other.m_plane;
if (clipped_count == 0)
{
return false; //Reject
}
clipped_count = other.clip_triangle(*this,clipped_points);
//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
if (clipped_count == 0 )
{
return false;//Reject
}
//find most deep interval face1
contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
if (contacts2.m_point_count == 0) return false; // too far
////check most dir for contacts
if (contacts2.m_penetration_depth<contacts1.m_penetration_depth)
{
contacts.copy_from(contacts2);
}
else
{
contacts.copy_from(contacts1);
}
return true;
////check most dir for contacts
if (contacts2.m_penetration_depth < contacts1.m_penetration_depth)
{
contacts.copy_from(contacts2);
}
else
{
contacts.copy_from(contacts1);
}
return true;
}
///class btTriangleShapeEx: public btTriangleShape
bool btTriangleShapeEx::overlap_test_conservative(const btTriangleShapeEx& other)
{
btScalar total_margin = getMargin() + other.getMargin();
btScalar total_margin = getMargin() + other.getMargin();
btVector4 plane0;
buildTriPlane(plane0);
btVector4 plane1;
other.buildTriPlane(plane1);
btVector4 plane0;
buildTriPlane(plane0);
btVector4 plane1;
other.buildTriPlane(plane1);
// classify points on other triangle
btScalar dis0 = bt_distance_point_plane(plane0,other.m_vertices1[0]) - total_margin;
// classify points on other triangle
btScalar dis0 = bt_distance_point_plane(plane0, other.m_vertices1[0]) - total_margin;
btScalar dis1 = bt_distance_point_plane(plane0,other.m_vertices1[1]) - total_margin;
btScalar dis1 = bt_distance_point_plane(plane0, other.m_vertices1[1]) - total_margin;
btScalar dis2 = bt_distance_point_plane(plane0,other.m_vertices1[2]) - total_margin;
btScalar dis2 = bt_distance_point_plane(plane0, other.m_vertices1[2]) - total_margin;
if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
// classify points on this triangle
dis0 = bt_distance_point_plane(plane1,m_vertices1[0]) - total_margin;
// classify points on this triangle
dis0 = bt_distance_point_plane(plane1, m_vertices1[0]) - total_margin;
dis1 = bt_distance_point_plane(plane1,m_vertices1[1]) - total_margin;
dis1 = bt_distance_point_plane(plane1, m_vertices1[1]) - total_margin;
dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - total_margin;
dis2 = bt_distance_point_plane(plane1, m_vertices1[2]) - total_margin;
if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false;
if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
return true;
return true;
}

71
src/BulletCollision/Gimpact/btTriangleShapeEx.h
View File

@@ -21,7 +21,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef GIMPACT_TRIANGLE_SHAPE_EX_H
#define GIMPACT_TRIANGLE_SHAPE_EX_H
@@ -31,16 +30,15 @@ subject to the following restrictions:
#include "btClipPolygon.h"
#include "btGeometryOperations.h"
#define MAX_TRI_CLIPPING 16
//! Structure for collision
struct GIM_TRIANGLE_CONTACT
{
btScalar m_penetration_depth;
int m_point_count;
btVector4 m_separating_normal;
btVector3 m_points[MAX_TRI_CLIPPING];
btScalar m_penetration_depth;
int m_point_count;
btVector4 m_separating_normal;
btVector3 m_points[MAX_TRI_CLIPPING];
SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT& other)
{
@@ -48,7 +46,7 @@ struct GIM_TRIANGLE_CONTACT
m_separating_normal = other.m_separating_normal;
m_point_count = other.m_point_count;
int i = m_point_count;
while(i--)
while (i--)
{
m_points[i] = other.m_points[i];
}
@@ -63,14 +61,11 @@ struct GIM_TRIANGLE_CONTACT
copy_from(other);
}
//! classify points that are closer
void merge_points(const btVector4 & plane,
btScalar margin, const btVector3 * points, int point_count);
//! classify points that are closer
void merge_points(const btVector4& plane,
btScalar margin, const btVector3* points, int point_count);
};
class btPrimitiveTriangle
{
public:
@@ -78,17 +73,15 @@ public:
btVector4 m_plane;
btScalar m_margin;
btScalar m_dummy;
btPrimitiveTriangle():m_margin(0.01f)
btPrimitiveTriangle() : m_margin(0.01f)
{
}
SIMD_FORCE_INLINE void buildTriPlane()
{
btVector3 normal = (m_vertices[1]-m_vertices[0]).cross(m_vertices[2]-m_vertices[0]);
btVector3 normal = (m_vertices[1] - m_vertices[0]).cross(m_vertices[2] - m_vertices[0]);
normal.normalize();
m_plane.setValue(normal[0],normal[1],normal[2],m_vertices[0].dot(normal));
m_plane.setValue(normal[0], normal[1], normal[2], m_vertices[0].dot(normal));
}
//! Test if triangles could collide
@@ -98,14 +91,14 @@ public:
/*!
\pre this triangle must have its plane calculated.
*/
SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4 &plane) const
{
const btVector3 & e0 = m_vertices[edge_index];
const btVector3 & e1 = m_vertices[(edge_index+1)%3];
bt_edge_plane(e0,e1,m_plane,plane);
}
SIMD_FORCE_INLINE void get_edge_plane(int edge_index, btVector4& plane) const
{
const btVector3& e0 = m_vertices[edge_index];
const btVector3& e1 = m_vertices[(edge_index + 1) % 3];
bt_edge_plane(e0, e1, m_plane, plane);
}
void applyTransform(const btTransform& t)
void applyTransform(const btTransform& t)
{
m_vertices[0] = t(m_vertices[0]);
m_vertices[1] = t(m_vertices[1]);
@@ -117,44 +110,41 @@ public:
\pre clipped_points must have MAX_TRI_CLIPPING size, and this triangle must have its plane calculated.
\return the number of clipped points
*/
int clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points );
int clip_triangle(btPrimitiveTriangle& other, btVector3* clipped_points);
//! Find collision using the clipping method
/*!
\pre this triangle and other must have their triangles calculated
*/
bool find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts);
bool find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_TRIANGLE_CONTACT& contacts);
};
//! Helper class for colliding Bullet Triangle Shapes
/*!
This class implements a better getAabb method than the previous btTriangleShape class
*/
class btTriangleShapeEx: public btTriangleShape
class btTriangleShapeEx : public btTriangleShape
{
public:
btTriangleShapeEx():btTriangleShape(btVector3(0,0,0),btVector3(0,0,0),btVector3(0,0,0))
btTriangleShapeEx() : btTriangleShape(btVector3(0, 0, 0), btVector3(0, 0, 0), btVector3(0, 0, 0))
{
}
btTriangleShapeEx(const btVector3& p0,const btVector3& p1,const btVector3& p2): btTriangleShape(p0,p1,p2)
btTriangleShapeEx(const btVector3& p0, const btVector3& p1, const btVector3& p2) : btTriangleShape(p0, p1, p2)
{
}
btTriangleShapeEx(const btTriangleShapeEx & other): btTriangleShape(other.m_vertices1[0],other.m_vertices1[1],other.m_vertices1[2])
btTriangleShapeEx(const btTriangleShapeEx& other) : btTriangleShape(other.m_vertices1[0], other.m_vertices1[1], other.m_vertices1[2])
{
}
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const
virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
btVector3 tv0 = t(m_vertices1[0]);
btVector3 tv1 = t(m_vertices1[1]);
btVector3 tv2 = t(m_vertices1[2]);
btAABB trianglebox(tv0,tv1,tv2,m_collisionMargin);
btAABB trianglebox(tv0, tv1, tv2, m_collisionMargin);
aabbMin = trianglebox.m_min;
aabbMax = trianglebox.m_max;
}
@@ -166,15 +156,14 @@ public:
m_vertices1[2] = t(m_vertices1[2]);
}
SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const
SIMD_FORCE_INLINE void buildTriPlane(btVector4& plane) const
{
btVector3 normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]);
btVector3 normal = (m_vertices1[1] - m_vertices1[0]).cross(m_vertices1[2] - m_vertices1[0]);
normal.normalize();
plane.setValue(normal[0],normal[1],normal[2],m_vertices1[0].dot(normal));
plane.setValue(normal[0], normal[1], normal[2], m_vertices1[0].dot(normal));
}
bool overlap_test_conservative(const btTriangleShapeEx& other);
};
#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H
#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H

300
src/BulletCollision/Gimpact/gim_array.h
View File

@@ -34,47 +34,46 @@ email: projectileman@yahoo.com
#include "gim_memory.h"
#define GIM_ARRAY_GROW_INCREMENT 2
#define GIM_ARRAY_GROW_FACTOR 2
//! Very simple array container with fast access and simd memory
template<typename T>
template <typename T>
class gim_array
{
public:
//! properties
//!@{
T *m_data;
GUINT m_size;
GUINT m_allocated_size;
//!@}
//! protected operations
//!@{
//! properties
//!@{
T* m_data;
GUINT m_size;
GUINT m_allocated_size;
//!@}
//! protected operations
//!@{
inline void destroyData()
inline void destroyData()
{
m_allocated_size = 0;
if(m_data==NULL) return;
m_allocated_size = 0;
if (m_data == NULL) return;
gim_free(m_data);
m_data = NULL;
}
inline bool resizeData(GUINT newsize)
{
if(newsize==0)
if (newsize == 0)
{
destroyData();
return true;
}
if(m_size>0)
if (m_size > 0)
{
m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T));
m_data = (T*)gim_realloc(m_data, m_size * sizeof(T), newsize * sizeof(T));
}
else
{
m_data = (T*)gim_alloc(newsize*sizeof(T));
m_data = (T*)gim_alloc(newsize * sizeof(T));
}
m_allocated_size = newsize;
return true;
@@ -82,243 +81,238 @@ public:
inline bool growingCheck()
{
if(m_allocated_size<=m_size)
if (m_allocated_size <= m_size)
{
GUINT requestsize = m_size;
m_size = m_allocated_size;
if(resizeData((requestsize+GIM_ARRAY_GROW_INCREMENT)*GIM_ARRAY_GROW_FACTOR)==false) return false;
GUINT requestsize = m_size;
m_size = m_allocated_size;
if (resizeData((requestsize + GIM_ARRAY_GROW_INCREMENT) * GIM_ARRAY_GROW_FACTOR) == false) return false;
}
return true;
}
//!@}
//! public operations
//!@{
inline bool reserve(GUINT size)
{
if(m_allocated_size>=size) return false;
return resizeData(size);
}
//!@}
//! public operations
//!@{
inline bool reserve(GUINT size)
{
if (m_allocated_size >= size) return false;
return resizeData(size);
}
inline void clear_range(GUINT start_range)
{
while(m_size>start_range)
{
m_data[--m_size].~T();
}
}
inline void clear_range(GUINT start_range)
{
while (m_size > start_range)
{
m_data[--m_size].~T();
}
}
inline void clear()
{
if(m_size==0)return;
clear_range(0);
}
inline void clear()
{
if (m_size == 0) return;
clear_range(0);
}
inline void clear_memory()
{
clear();
destroyData();
}
inline void clear_memory()
{
clear();
destroyData();
}
gim_array()
{
m_data = 0;
m_size = 0;
m_allocated_size = 0;
}
gim_array()
{
m_data = 0;
m_size = 0;
m_allocated_size = 0;
}
gim_array(GUINT reservesize)
{
m_data = 0;
m_size = 0;
gim_array(GUINT reservesize)
{
m_data = 0;
m_size = 0;
m_allocated_size = 0;
reserve(reservesize);
}
m_allocated_size = 0;
reserve(reservesize);
}
~gim_array()
{
clear_memory();
}
~gim_array()
{
clear_memory();
}
inline GUINT size() const
{
return m_size;
}
inline GUINT size() const
{
return m_size;
}
inline GUINT max_size() const
{
return m_allocated_size;
}
inline GUINT max_size() const
{
return m_allocated_size;
}
inline T & operator[](size_t i)
inline T& operator[](size_t i)
{
return m_data[i];
}
inline const T & operator[](size_t i) const
inline const T& operator[](size_t i) const
{
return m_data[i];
}
inline T * pointer(){ return m_data;}
inline const T * pointer() const
{ return m_data;}
inline T* pointer() { return m_data; }
inline const T* pointer() const
{
return m_data;
}
inline T * get_pointer_at(GUINT i)
inline T* get_pointer_at(GUINT i)
{
return m_data + i;
}
inline const T * get_pointer_at(GUINT i) const
inline const T* get_pointer_at(GUINT i) const
{
return m_data + i;
}
inline T & at(GUINT i)
inline T& at(GUINT i)
{
return m_data[i];
}
inline const T & at(GUINT i) const
inline const T& at(GUINT i) const
{
return m_data[i];
}
inline T & front()
inline T& front()
{
return *m_data;
}
inline const T & front() const
inline const T& front() const
{
return *m_data;
}
inline T & back()
inline T& back()
{
return m_data[m_size-1];
return m_data[m_size - 1];
}
inline const T & back() const
inline const T& back() const
{
return m_data[m_size-1];
return m_data[m_size - 1];
}
inline void swap(GUINT i, GUINT j)
{
gim_swap_elements(m_data,i,j);
gim_swap_elements(m_data, i, j);
}
inline void push_back(const T & obj)
inline void push_back(const T& obj)
{
this->growingCheck();
m_data[m_size] = obj;
m_size++;
this->growingCheck();
m_data[m_size] = obj;
m_size++;
}
//!Simply increase the m_size, doesn't call the new element constructor
inline void push_back_mem()
{
this->growingCheck();
m_size++;
this->growingCheck();
m_size++;
}
inline void push_back_memcpy(const T & obj)
inline void push_back_memcpy(const T& obj)
{
this->growingCheck();
gim_simd_memcpy(&m_data[m_size],&obj,sizeof(T));
m_size++;
this->growingCheck();
gim_simd_memcpy(&m_data[m_size], &obj, sizeof(T));
m_size++;
}
inline void pop_back()
{
m_size--;
m_data[m_size].~T();
m_size--;
m_data[m_size].~T();
}
//!Simply decrease the m_size, doesn't call the deleted element destructor
inline void pop_back_mem()
{
m_size--;
m_size--;
}
//! fast erase
//! fast erase
inline void erase(GUINT index)
{
if(index<m_size-1)
{
swap(index,m_size-1);
}
pop_back();
if (index < m_size - 1)
{
swap(index, m_size - 1);
}
pop_back();
}
inline void erase_sorted_mem(GUINT index)
{
m_size--;
for(GUINT i = index;i<m_size;i++)
{
gim_simd_memcpy(m_data+i,m_data+i+1,sizeof(T));
}
m_size--;
for (GUINT i = index; i < m_size; i++)
{
gim_simd_memcpy(m_data + i, m_data + i + 1, sizeof(T));
}
}
inline void erase_sorted(GUINT index)
{
m_data[index].~T();
erase_sorted_mem(index);
m_data[index].~T();
erase_sorted_mem(index);
}
inline void insert_mem(GUINT index)
{
this->growingCheck();
for(GUINT i = m_size;i>index;i--)
{
gim_simd_memcpy(m_data+i,m_data+i-1,sizeof(T));
}
m_size++;
this->growingCheck();
for (GUINT i = m_size; i > index; i--)
{
gim_simd_memcpy(m_data + i, m_data + i - 1, sizeof(T));
}
m_size++;
}
inline void insert(const T & obj,GUINT index)
inline void insert(const T& obj, GUINT index)
{
insert_mem(index);
m_data[index] = obj;
insert_mem(index);
m_data[index] = obj;
}
inline void resize(GUINT size, bool call_constructor = true, const T& fillData=T())
inline void resize(GUINT size, bool call_constructor = true, const T& fillData = T())
{
if(size>m_size)
{
reserve(size);
if(call_constructor)
{
while(m_size<size)
{
m_data[m_size] = fillData;
m_size++;
}
}
else
{
m_size = size;
}
}
else if(size<m_size)
{
if(call_constructor) clear_range(size);
m_size = size;
}
if (size > m_size)
{
reserve(size);
if (call_constructor)
{
while (m_size < size)
{
m_data[m_size] = fillData;
m_size++;
}
}
else
{
m_size = size;
}
}
else if (size < m_size)
{
if (call_constructor) clear_range(size);
m_size = size;
}
}
inline void refit()
{
resizeData(m_size);
resizeData(m_size);
}
};
#endif // GIM_CONTAINERS_H_INCLUDED
#endif // GIM_CONTAINERS_H_INCLUDED

530
src/BulletCollision/Gimpact/gim_basic_geometry_operations.h
View File

@@ -35,12 +35,8 @@ email: projectileman@yahoo.com
-----------------------------------------------------------------------------
*/
#include "gim_linear_math.h"
#ifndef PLANEDIREPSILON
#define PLANEDIREPSILON 0.0000001f
#endif
@@ -49,77 +45,82 @@ email: projectileman@yahoo.com
#define PARALELENORMALS 0.000001f
#endif
#define TRIANGLE_NORMAL(v1,v2,v3,n)\
{\
vec3f _dif1,_dif2;\
VEC_DIFF(_dif1,v2,v1);\
VEC_DIFF(_dif2,v3,v1);\
VEC_CROSS(n,_dif1,_dif2);\
VEC_NORMALIZE(n);\
}\
#define TRIANGLE_NORMAL(v1, v2, v3, n) \
{ \
vec3f _dif1, _dif2; \
VEC_DIFF(_dif1, v2, v1); \
VEC_DIFF(_dif2, v3, v1); \
VEC_CROSS(n, _dif1, _dif2); \
VEC_NORMALIZE(n); \
}
#define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
vec3f _dif1,_dif2; \
VEC_DIFF(_dif1,v2,v1); \
VEC_DIFF(_dif2,v3,v1); \
VEC_CROSS(n,_dif1,_dif2); \
}\
#define TRIANGLE_NORMAL_FAST(v1, v2, v3, n) \
{ \
vec3f _dif1, _dif2; \
VEC_DIFF(_dif1, v2, v1); \
VEC_DIFF(_dif2, v3, v1); \
VEC_CROSS(n, _dif1, _dif2); \
}
/// plane is a vec4f
#define TRIANGLE_PLANE(v1,v2,v3,plane) {\
TRIANGLE_NORMAL(v1,v2,v3,plane);\
plane[3] = VEC_DOT(v1,plane);\
}\
#define TRIANGLE_PLANE(v1, v2, v3, plane) \
{ \
TRIANGLE_NORMAL(v1, v2, v3, plane); \
plane[3] = VEC_DOT(v1, plane); \
}
/// plane is a vec4f
#define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
plane[3] = VEC_DOT(v1,plane);\
}\
#define TRIANGLE_PLANE_FAST(v1, v2, v3, plane) \
{ \
TRIANGLE_NORMAL_FAST(v1, v2, v3, plane); \
plane[3] = VEC_DOT(v1, plane); \
}
/// Calc a plane from an edge an a normal. plane is a vec4f
#define EDGE_PLANE(e1,e2,n,plane) {\
vec3f _dif; \
VEC_DIFF(_dif,e2,e1); \
VEC_CROSS(plane,_dif,n); \
VEC_NORMALIZE(plane); \
plane[3] = VEC_DOT(e1,plane);\
}\
#define EDGE_PLANE(e1, e2, n, plane) \
{ \
vec3f _dif; \
VEC_DIFF(_dif, e2, e1); \
VEC_CROSS(plane, _dif, n); \
VEC_NORMALIZE(plane); \
plane[3] = VEC_DOT(e1, plane); \
}
#define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])
#define DISTANCE_PLANE_POINT(plane, point) (VEC_DOT(plane, point) - plane[3])
#define PROJECT_POINT_PLANE(point,plane,projected) {\
GREAL _dis;\
_dis = DISTANCE_PLANE_POINT(plane,point);\
VEC_SCALE(projected,-_dis,plane);\
VEC_SUM(projected,projected,point); \
}\
#define PROJECT_POINT_PLANE(point, plane, projected) \
{ \
GREAL _dis; \
_dis = DISTANCE_PLANE_POINT(plane, point); \
VEC_SCALE(projected, -_dis, plane); \
VEC_SUM(projected, projected, point); \
}
//! Verifies if a point is in the plane hull
template<typename CLASS_POINT,typename CLASS_PLANE>
template <typename CLASS_POINT, typename CLASS_PLANE>
SIMD_FORCE_INLINE bool POINT_IN_HULL(
const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
const CLASS_POINT &point, const CLASS_PLANE *planes, GUINT plane_count)
{
GREAL _dis;
for (GUINT _i = 0;_i< plane_count;++_i)
for (GUINT _i = 0; _i < plane_count; ++_i)
{
_dis = DISTANCE_PLANE_POINT(planes[_i],point);
if(_dis>0.0f) return false;
_dis = DISTANCE_PLANE_POINT(planes[_i], point);
if (_dis > 0.0f) return false;
}
return true;
}
template<typename CLASS_POINT,typename CLASS_PLANE>
template <typename CLASS_POINT, typename CLASS_PLANE>
SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
const CLASS_POINT& s1,
const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
const CLASS_POINT &s1,
const CLASS_POINT &s2, const CLASS_PLANE &plane, CLASS_POINT &clipped)
{
GREAL _dis1,_dis2;
_dis1 = DISTANCE_PLANE_POINT(plane,s1);
VEC_DIFF(clipped,s2,s1);
_dis2 = VEC_DOT(clipped,plane);
VEC_SCALE(clipped,-_dis1/_dis2,clipped);
VEC_SUM(clipped,clipped,s1);
GREAL _dis1, _dis2;
_dis1 = DISTANCE_PLANE_POINT(plane, s1);
VEC_DIFF(clipped, s2, s1);
_dis2 = VEC_DOT(clipped, plane);
VEC_SCALE(clipped, -_dis1 / _dis2, clipped);
VEC_SUM(clipped, clipped, s1);
}
enum ePLANE_INTERSECTION_TYPE
@@ -152,30 +153,30 @@ intersection type must have the following values
</ul>
*/
template<typename CLASS_POINT,typename CLASS_PLANE>
template <typename CLASS_POINT, typename CLASS_PLANE>
SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
const CLASS_POINT& s1,
const CLASS_POINT &s1,
const CLASS_POINT &s2,
const CLASS_PLANE &plane,CLASS_POINT &clipped)
const CLASS_PLANE &plane, CLASS_POINT &clipped)
{
GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
GREAL _dis1 = DISTANCE_PLANE_POINT(plane, s1);
GREAL _dis2 = DISTANCE_PLANE_POINT(plane, s2);
if (_dis1 > -G_EPSILON && _dis2 > -G_EPSILON)
{
if(_dis1<_dis2) return G_FRONT_PLANE_S1;
return G_FRONT_PLANE_S2;
if (_dis1 < _dis2) return G_FRONT_PLANE_S1;
return G_FRONT_PLANE_S2;
}
else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
else if (_dis1 < G_EPSILON && _dis2 < G_EPSILON)
{
if(_dis1>_dis2) return G_BACK_PLANE_S1;
return G_BACK_PLANE_S2;
if (_dis1 > _dis2) return G_BACK_PLANE_S1;
return G_BACK_PLANE_S2;
}
VEC_DIFF(clipped,s2,s1);
_dis2 = VEC_DOT(clipped,plane);
VEC_SCALE(clipped,-_dis1/_dis2,clipped);
VEC_SUM(clipped,clipped,s1);
if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
VEC_DIFF(clipped, s2, s1);
_dis2 = VEC_DOT(clipped, plane);
VEC_SCALE(clipped, -_dis1 / _dis2, clipped);
VEC_SUM(clipped, clipped, s1);
if (_dis1 < _dis2) return G_COLLIDE_PLANE_S1;
return G_COLLIDE_PLANE_S2;
}
@@ -194,43 +195,42 @@ intersection_type must have the following values
<li> 5 : Segment collides plane, s2 in back
</ul>
*/
template<typename CLASS_POINT,typename CLASS_PLANE>
template <typename CLASS_POINT, typename CLASS_PLANE>
SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
const CLASS_POINT& s1,
const CLASS_POINT &s1,
const CLASS_POINT &s2,
const CLASS_PLANE &plane,
CLASS_POINT &clipped1,CLASS_POINT &clipped2)
CLASS_POINT &clipped1, CLASS_POINT &clipped2)
{
eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
switch(intersection_type)
eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1, s2, plane, clipped1);
switch (intersection_type)
{
case G_FRONT_PLANE_S1:
VEC_COPY(clipped1,s1);
VEC_COPY(clipped2,s2);
break;
case G_FRONT_PLANE_S2:
VEC_COPY(clipped1,s2);
VEC_COPY(clipped2,s1);
break;
case G_BACK_PLANE_S1:
VEC_COPY(clipped1,s1);
VEC_COPY(clipped2,s2);
break;
case G_BACK_PLANE_S2:
VEC_COPY(clipped1,s2);
VEC_COPY(clipped2,s1);
break;
case G_COLLIDE_PLANE_S1:
VEC_COPY(clipped2,s1);
break;
case G_COLLIDE_PLANE_S2:
VEC_COPY(clipped2,s2);
break;
case G_FRONT_PLANE_S1:
VEC_COPY(clipped1, s1);
VEC_COPY(clipped2, s2);
break;
case G_FRONT_PLANE_S2:
VEC_COPY(clipped1, s2);
VEC_COPY(clipped2, s1);
break;
case G_BACK_PLANE_S1:
VEC_COPY(clipped1, s1);
VEC_COPY(clipped2, s2);
break;
case G_BACK_PLANE_S2:
VEC_COPY(clipped1, s2);
VEC_COPY(clipped2, s1);
break;
case G_COLLIDE_PLANE_S1:
VEC_COPY(clipped2, s1);
break;
case G_COLLIDE_PLANE_S2:
VEC_COPY(clipped2, s2);
break;
}
return intersection_type;
}
//! Finds the 2 smallest cartesian coordinates of a plane normal
#define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
@@ -239,23 +239,23 @@ SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
It uses the PLANEDIREPSILON constant.
*/
template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
template <typename T, typename CLASS_POINT, typename CLASS_PLANE>
SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
const CLASS_PLANE & plane,
const CLASS_POINT & vDir,
const CLASS_POINT & vPoint,
CLASS_POINT & pout,T &tparam)
const CLASS_PLANE &plane,
const CLASS_POINT &vDir,
const CLASS_POINT &vPoint,
CLASS_POINT &pout, T &tparam)
{
GREAL _dis,_dotdir;
_dotdir = VEC_DOT(plane,vDir);
if(_dotdir<PLANEDIREPSILON)
GREAL _dis, _dotdir;
_dotdir = VEC_DOT(plane, vDir);
if (_dotdir < PLANEDIREPSILON)
{
return false;
return false;
}
_dis = DISTANCE_PLANE_POINT(plane,vPoint);
tparam = -_dis/_dotdir;
VEC_SCALE(pout,tparam,vDir);
VEC_SUM(pout,vPoint,pout);
_dis = DISTANCE_PLANE_POINT(plane, vPoint);
tparam = -_dis / _dotdir;
VEC_SCALE(pout, tparam, vDir);
VEC_SUM(pout, vPoint, pout);
return true;
}
@@ -266,39 +266,39 @@ SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
-1 if the ray collides in front
-2 if the ray collides in back
*/
template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
template <typename T, typename CLASS_POINT, typename CLASS_PLANE>
SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
const CLASS_PLANE & plane,
const CLASS_POINT & vDir,
const CLASS_POINT & vPoint,
CLASS_POINT & pout,
const CLASS_PLANE &plane,
const CLASS_POINT &vDir,
const CLASS_POINT &vPoint,
CLASS_POINT &pout,
T &tparam,
T tmin, T tmax)
{
GREAL _dis,_dotdir;
_dotdir = VEC_DOT(plane,vDir);
if(btFabs(_dotdir)<PLANEDIREPSILON)
GREAL _dis, _dotdir;
_dotdir = VEC_DOT(plane, vDir);
if (btFabs(_dotdir) < PLANEDIREPSILON)
{
tparam = tmax;
return 0;
return 0;
}
_dis = DISTANCE_PLANE_POINT(plane,vPoint);
char returnvalue = _dis<0.0f?2:1;
tparam = -_dis/_dotdir;
_dis = DISTANCE_PLANE_POINT(plane, vPoint);
char returnvalue = _dis < 0.0f ? 2 : 1;
tparam = -_dis / _dotdir;
if(tparam<tmin)
if (tparam < tmin)
{
returnvalue = 0;
tparam = tmin;
}
else if(tparam>tmax)
else if (tparam > tmax)
{
returnvalue = 0;
tparam = tmax;
}
VEC_SCALE(pout,tparam,vDir);
VEC_SUM(pout,vPoint,pout);
VEC_SCALE(pout, tparam, vDir);
VEC_SUM(pout, vPoint, pout);
return returnvalue;
}
@@ -312,24 +312,24 @@ SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
\return true if the planes intersect, 0 if paralell.
*/
template<typename CLASS_POINT,typename CLASS_PLANE>
template <typename CLASS_POINT, typename CLASS_PLANE>
SIMD_FORCE_INLINE bool INTERSECT_PLANES(
const CLASS_PLANE &p1,
const CLASS_PLANE &p2,
CLASS_POINT &p,
CLASS_POINT &d)
const CLASS_PLANE &p1,
const CLASS_PLANE &p2,
CLASS_POINT &p,
CLASS_POINT &d)
{
VEC_CROSS(d,p1,p2);
GREAL denom = VEC_DOT(d, d);
if(GIM_IS_ZERO(denom)) return false;
VEC_CROSS(d, p1, p2);
GREAL denom = VEC_DOT(d, d);
if (GIM_IS_ZERO(denom)) return false;
vec3f _n;
_n[0]=p1[3]*p2[0] - p2[3]*p1[0];
_n[1]=p1[3]*p2[1] - p2[3]*p1[1];
_n[2]=p1[3]*p2[2] - p2[3]*p1[2];
VEC_CROSS(p,_n,d);
p[0]/=denom;
p[1]/=denom;
p[2]/=denom;
_n[0] = p1[3] * p2[0] - p2[3] * p1[0];
_n[1] = p1[3] * p2[1] - p2[3] * p1[1];
_n[2] = p1[3] * p2[2] - p2[3] * p1[2];
VEC_CROSS(p, _n, d);
p[0] /= denom;
p[1] /= denom;
p[2] /= denom;
return true;
}
@@ -337,32 +337,31 @@ SIMD_FORCE_INLINE bool INTERSECT_PLANES(
/*! Finds the closest point(cp) to (v) on a segment (e1,e2)
*/
template<typename CLASS_POINT>
template <typename CLASS_POINT>
SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
CLASS_POINT & cp, const CLASS_POINT & v,
const CLASS_POINT &e1,const CLASS_POINT &e2)
CLASS_POINT &cp, const CLASS_POINT &v,
const CLASS_POINT &e1, const CLASS_POINT &e2)
{
vec3f _n;
VEC_DIFF(_n,e2,e1);
VEC_DIFF(cp,v,e1);
vec3f _n;
VEC_DIFF(_n, e2, e1);
VEC_DIFF(cp, v, e1);
GREAL _scalar = VEC_DOT(cp, _n);
_scalar/= VEC_DOT(_n, _n);
if(_scalar <0.0f)
_scalar /= VEC_DOT(_n, _n);
if (_scalar < 0.0f)
{
VEC_COPY(cp,e1);
VEC_COPY(cp, e1);
}
else if(_scalar >1.0f)
else if (_scalar > 1.0f)
{
VEC_COPY(cp,e2);
VEC_COPY(cp, e2);
}
else
{
VEC_SCALE(cp,_scalar,_n);
VEC_SUM(cp,cp,e1);
VEC_SCALE(cp, _scalar, _n);
VEC_SUM(cp, cp, e1);
}
}
/*! \brief Finds the line params where these lines intersect.
\param dir1 Direction of line 1
@@ -374,118 +373,114 @@ SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
\param dointersect 0 if the lines won't intersect, else 1
*/
template<typename T,typename CLASS_POINT>
template <typename T, typename CLASS_POINT>
SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
const CLASS_POINT & dir1,
CLASS_POINT & point1,
const CLASS_POINT & dir2,
CLASS_POINT & point2,
T& t1,T& t2)
const CLASS_POINT &dir1,
CLASS_POINT &point1,
const CLASS_POINT &dir2,
CLASS_POINT &point2,
T &t1, T &t2)
{
GREAL det;
GREAL e1e1 = VEC_DOT(dir1,dir1);
GREAL e1e2 = VEC_DOT(dir1,dir2);
GREAL e2e2 = VEC_DOT(dir2,dir2);
GREAL det;
GREAL e1e1 = VEC_DOT(dir1, dir1);
GREAL e1e2 = VEC_DOT(dir1, dir2);
GREAL e2e2 = VEC_DOT(dir2, dir2);
vec3f p1p2;
VEC_DIFF(p1p2,point1,point2);
GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
det = e1e2*e1e2 - e1e1*e2e2;
if(GIM_IS_ZERO(det)) return false;
t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
VEC_DIFF(p1p2, point1, point2);
GREAL p1p2e1 = VEC_DOT(p1p2, dir1);
GREAL p1p2e2 = VEC_DOT(p1p2, dir2);
det = e1e2 * e1e2 - e1e1 * e2e2;
if (GIM_IS_ZERO(det)) return false;
t1 = (e1e2 * p1p2e2 - e2e2 * p1p2e1) / det;
t2 = (e1e1 * p1p2e2 - e1e2 * p1p2e1) / det;
return true;
}
//! Find closest points on segments
template<typename CLASS_POINT>
template <typename CLASS_POINT>
SIMD_FORCE_INLINE void SEGMENT_COLLISION(
const CLASS_POINT & vA1,
const CLASS_POINT & vA2,
const CLASS_POINT & vB1,
const CLASS_POINT & vB2,
CLASS_POINT & vPointA,
CLASS_POINT & vPointB)
const CLASS_POINT &vA1,
const CLASS_POINT &vA2,
const CLASS_POINT &vB1,
const CLASS_POINT &vB2,
CLASS_POINT &vPointA,
CLASS_POINT &vPointB)
{
CLASS_POINT _AD,_BD,n;
vec4f _M;//plane
VEC_DIFF(_AD,vA2,vA1);
VEC_DIFF(_BD,vB2,vB1);
VEC_CROSS(n,_AD,_BD);
GREAL _tp = VEC_DOT(n,n);
if(_tp<G_EPSILON)//ARE PARALELE
{
//project B over A
bool invert_b_order = false;
_M[0] = VEC_DOT(vB1,_AD);
_M[1] = VEC_DOT(vB2,_AD);
if(_M[0]>_M[1])
{
invert_b_order = true;
GIM_SWAP_NUMBERS(_M[0],_M[1]);
}
_M[2] = VEC_DOT(vA1,_AD);
_M[3] = VEC_DOT(vA2,_AD);
//mid points
n[0] = (_M[0]+_M[1])*0.5f;
n[1] = (_M[2]+_M[3])*0.5f;
CLASS_POINT _AD, _BD, n;
vec4f _M; //plane
VEC_DIFF(_AD, vA2, vA1);
VEC_DIFF(_BD, vB2, vB1);
VEC_CROSS(n, _AD, _BD);
GREAL _tp = VEC_DOT(n, n);
if (_tp < G_EPSILON) //ARE PARALELE
{
//project B over A
bool invert_b_order = false;
_M[0] = VEC_DOT(vB1, _AD);
_M[1] = VEC_DOT(vB2, _AD);
if (_M[0] > _M[1])
{
invert_b_order = true;
GIM_SWAP_NUMBERS(_M[0], _M[1]);
}
_M[2] = VEC_DOT(vA1, _AD);
_M[3] = VEC_DOT(vA2, _AD);
//mid points
n[0] = (_M[0] + _M[1]) * 0.5f;
n[1] = (_M[2] + _M[3]) * 0.5f;
if(n[0]<n[1])
{
if(_M[1]<_M[2])
{
vPointB = invert_b_order?vB1:vB2;
vPointA = vA1;
}
else if(_M[1]<_M[3])
{
vPointB = invert_b_order?vB1:vB2;
CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
}
else
{
vPointA = vA2;
CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
}
}
else
{
if(_M[3]<_M[0])
{
vPointB = invert_b_order?vB2:vB1;
vPointA = vA2;
}
else if(_M[3]<_M[1])
{
vPointA = vA2;
CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
}
else
{
vPointB = invert_b_order?vB1:vB2;
CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
}
}
return;
}
if (n[0] < n[1])
{
if (_M[1] < _M[2])
{
vPointB = invert_b_order ? vB1 : vB2;
vPointA = vA1;
}
else if (_M[1] < _M[3])
{
vPointB = invert_b_order ? vB1 : vB2;
CLOSEST_POINT_ON_SEGMENT(vPointA, vPointB, vA1, vA2);
}
else
{
vPointA = vA2;
CLOSEST_POINT_ON_SEGMENT(vPointB, vPointA, vB1, vB2);
}
}
else
{
if (_M[3] < _M[0])
{
vPointB = invert_b_order ? vB2 : vB1;
vPointA = vA2;
}
else if (_M[3] < _M[1])
{
vPointA = vA2;
CLOSEST_POINT_ON_SEGMENT(vPointB, vPointA, vB1, vB2);
}
else
{
vPointB = invert_b_order ? vB1 : vB2;
CLOSEST_POINT_ON_SEGMENT(vPointA, vPointB, vA1, vA2);
}
}
return;
}
VEC_CROSS(_M, n, _BD);
_M[3] = VEC_DOT(_M, vB1);
VEC_CROSS(_M,n,_BD);
_M[3] = VEC_DOT(_M,vB1);
LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
/*Closest point on segment*/
VEC_DIFF(vPointB,vPointA,vB1);
LINE_PLANE_COLLISION(_M, _AD, vA1, vPointA, _tp, btScalar(0), btScalar(1));
/*Closest point on segment*/
VEC_DIFF(vPointB, vPointA, vB1);
_tp = VEC_DOT(vPointB, _BD);
_tp/= VEC_DOT(_BD, _BD);
_tp = GIM_CLAMP(_tp,0.0f,1.0f);
VEC_SCALE(vPointB,_tp,_BD);
VEC_SUM(vPointB,vPointB,vB1);
_tp /= VEC_DOT(_BD, _BD);
_tp = GIM_CLAMP(_tp, 0.0f, 1.0f);
VEC_SCALE(vPointB, _tp, _BD);
VEC_SUM(vPointB, vPointB, vB1);
}
//! Line box intersection in one dimension
/*!
@@ -497,37 +492,36 @@ SIMD_FORCE_INLINE void SEGMENT_COLLISION(
*\param tlast the maximum projection. Assign to INFINITY at first.
*\return true if there is an intersection.
*/
template<typename T>
SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
template <typename T>
SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir, T bmin, T bmax, T &tfirst, T &tlast)
{
if(GIM_IS_ZERO(dir))
if (GIM_IS_ZERO(dir))
{
return !(pos < bmin || pos > bmax);
return !(pos < bmin || pos > bmax);
}
GREAL a0 = (bmin - pos) / dir;
GREAL a1 = (bmax - pos) / dir;
if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1);
if (a0 > a1) GIM_SWAP_NUMBERS(a0, a1);
tfirst = GIM_MAX(a0, tfirst);
tlast = GIM_MIN(a1, tlast);
if (tlast < tfirst) return false;
return true;
}
//! Sorts 3 componets
template<typename T>
template <typename T>
SIMD_FORCE_INLINE void SORT_3_INDICES(
const T * values,
GUINT * order_indices)
const T *values,
GUINT *order_indices)
{
//get minimum
order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
//get second and third
GUINT i0 = (order_indices[0] + 1)%3;
GUINT i1 = (i0 + 1)%3;
GUINT i0 = (order_indices[0] + 1) % 3;
GUINT i1 = (i0 + 1) % 3;
if(values[i0] < values[i1])
if (values[i0] < values[i1])
{
order_indices[1] = i0;
order_indices[2] = i1;
@@ -539,8 +533,4 @@ SIMD_FORCE_INLINE void SORT_3_INDICES(
}
}
#endif // GIM_VECTOR_H_INCLUDED
#endif // GIM_VECTOR_H_INCLUDED

50
src/BulletCollision/Gimpact/gim_bitset.h
View File

@@ -34,34 +34,32 @@ email: projectileman@yahoo.com
#include "gim_array.h"
#define GUINT_BIT_COUNT 32
#define GUINT_EXPONENT 5
class gim_bitset
{
public:
gim_array<GUINT> m_container;
gim_array<GUINT> m_container;
gim_bitset()
{
gim_bitset()
{
}
}
gim_bitset(GUINT bits_count)
{
resize(bits_count);
}
gim_bitset(GUINT bits_count)
{
resize(bits_count);
}
~gim_bitset()
{
}
~gim_bitset()
{
}
inline bool resize(GUINT newsize)
{
GUINT oldsize = m_container.size();
m_container.resize(newsize/GUINT_BIT_COUNT + 1,false);
while(oldsize<m_container.size())
m_container.resize(newsize / GUINT_BIT_COUNT + 1, false);
while (oldsize < m_container.size())
{
m_container[oldsize] = 0;
}
@@ -70,12 +68,12 @@ public:
inline GUINT size()
{
return m_container.size()*GUINT_BIT_COUNT;
return m_container.size() * GUINT_BIT_COUNT;
}
inline void set_all()
{
for(GUINT i = 0;i<m_container.size();++i)
for (GUINT i = 0; i < m_container.size(); ++i)
{
m_container[i] = 0xffffffff;
}
@@ -83,7 +81,7 @@ public:
inline void clear_all()
{
for(GUINT i = 0;i<m_container.size();++i)
for (GUINT i = 0; i < m_container.size(); ++i)
{
m_container[i] = 0;
}
@@ -91,33 +89,29 @@ public:
inline void set(GUINT bit_index)
{
if(bit_index>=size())
if (bit_index >= size())
{
resize(bit_index);
}
m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT-1)));
m_container[bit_index >> GUINT_EXPONENT] |= (1 << (bit_index & (GUINT_BIT_COUNT - 1)));
}
///Return 0 or 1
inline char get(GUINT bit_index)
{
if(bit_index>=size())
if (bit_index >= size())
{
return 0;
}
char value = m_container[bit_index >> GUINT_EXPONENT] &
(1 << (bit_index & (GUINT_BIT_COUNT-1)));
(1 << (bit_index & (GUINT_BIT_COUNT - 1)));
return value;
}
inline void clear(GUINT bit_index)
{
m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT-1)));
m_container[bit_index >> GUINT_EXPONENT] &= ~(1 << (bit_index & (GUINT_BIT_COUNT - 1)));
}
};
#endif // GIM_CONTAINERS_H_INCLUDED
#endif // GIM_CONTAINERS_H_INCLUDED

401
src/BulletCollision/Gimpact/gim_box_collision.h
View File

@@ -35,8 +35,6 @@ email: projectileman@yahoo.com
#include "gim_basic_geometry_operations.h"
#include "LinearMath/btTransform.h"
//SIMD_FORCE_INLINE bool test_cross_edge_box(
// const btVector3 & edge,
// const btVector3 & absolute_edge,
@@ -99,52 +97,50 @@ email: projectileman@yahoo.com
#ifndef TEST_CROSS_EDGE_BOX_MCR
#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
{\
const btScalar dir0 = -edge[i_dir_0];\
const btScalar dir1 = edge[i_dir_1];\
btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
if(pmin>pmax)\
{\
GIM_SWAP_NUMBERS(pmin,pmax); \
}\
const btScalar abs_dir0 = absolute_edge[i_dir_0];\
const btScalar abs_dir1 = absolute_edge[i_dir_1];\
const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
if(pmin>rad || -rad>pmax) return false;\
}\
#define TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, i_dir_0, i_dir_1, i_comp_0, i_comp_1) \
{ \
const btScalar dir0 = -edge[i_dir_0]; \
const btScalar dir1 = edge[i_dir_1]; \
btScalar pmin = pointa[i_comp_0] * dir0 + pointa[i_comp_1] * dir1; \
btScalar pmax = pointb[i_comp_0] * dir0 + pointb[i_comp_1] * dir1; \
if (pmin > pmax) \
{ \
GIM_SWAP_NUMBERS(pmin, pmax); \
} \
const btScalar abs_dir0 = absolute_edge[i_dir_0]; \
const btScalar abs_dir1 = absolute_edge[i_dir_1]; \
const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1; \
if (pmin > rad || -rad > pmax) return false; \
}
#endif
#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
{\
TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
}\
#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
{\
TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
}\
#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
{\
TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
}\
#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \
{ \
TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 2, 1, 1, 2); \
}
#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \
{ \
TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 0, 2, 2, 0); \
}
#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge, absolute_edge, pointa, pointb, _extend) \
{ \
TEST_CROSS_EDGE_BOX_MCR(edge, absolute_edge, pointa, pointb, _extend, 1, 0, 0, 1); \
}
//! Class for transforming a model1 to the space of model0
class GIM_BOX_BOX_TRANSFORM_CACHE
{
public:
btVector3 m_T1to0;//!< Transforms translation of model1 to model 0
btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal to R0' * R1
btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
btVector3 m_T1to0; //!< Transforms translation of model1 to model 0
btMatrix3x3 m_R1to0; //!< Transforms Rotation of model1 to model 0, equal to R0' * R1
btMatrix3x3 m_AR; //!< Absolute value of m_R1to0
SIMD_FORCE_INLINE void calc_absolute_matrix()
{
static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
static const btVector3 vepsi(1e-6f, 1e-6f, 1e-6f);
m_AR[0] = vepsi + m_R1to0[0].absolute();
m_AR[1] = vepsi + m_R1to0[1].absolute();
m_AR[2] = vepsi + m_R1to0[2].absolute();
@@ -154,42 +150,40 @@ public:
{
}
GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0)
GIM_BOX_BOX_TRANSFORM_CACHE(mat4f trans1_to_0)
{
COPY_MATRIX_3X3(m_R1to0,trans1_to_0)
MAT_GET_TRANSLATION(trans1_to_0,m_T1to0)
COPY_MATRIX_3X3(m_R1to0, trans1_to_0)
MAT_GET_TRANSLATION(trans1_to_0, m_T1to0)
calc_absolute_matrix();
}
//! Calc the transformation relative 1 to 0. Inverts matrics by transposing
SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform &trans0, const btTransform &trans1)
{
m_R1to0 = trans0.getBasis().transpose();
m_T1to0 = m_R1to0 * (-trans0.getOrigin());
m_T1to0 += m_R1to0*trans1.getOrigin();
m_T1to0 += m_R1to0 * trans1.getOrigin();
m_R1to0 *= trans1.getBasis();
calc_absolute_matrix();
}
//! Calcs the full invertion of the matrices. Useful for scaling matrices
SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform &trans0, const btTransform &trans1)
{
m_R1to0 = trans0.getBasis().inverse();
m_T1to0 = m_R1to0 * (-trans0.getOrigin());
m_T1to0 += m_R1to0*trans1.getOrigin();
m_T1to0 += m_R1to0 * trans1.getOrigin();
m_R1to0 *= trans1.getBasis();
calc_absolute_matrix();
}
SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point)
SIMD_FORCE_INLINE btVector3 transform(const btVector3 &point)
{
return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0;
return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0;
}
};
@@ -205,34 +199,34 @@ public:
btVector3 m_max;
GIM_AABB()
{}
GIM_AABB(const btVector3 & V1,
const btVector3 & V2,
const btVector3 & V3)
{
m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
}
GIM_AABB(const btVector3 & V1,
const btVector3 & V2,
const btVector3 & V3,
GIM_AABB(const btVector3 &V1,
const btVector3 &V2,
const btVector3 &V3)
{
m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]);
m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]);
m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]);
m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]);
m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]);
m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]);
}
GIM_AABB(const btVector3 &V1,
const btVector3 &V2,
const btVector3 &V3,
GREAL margin)
{
m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]);
m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]);
m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]);
m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]);
m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]);
m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]);
m_min[0] -= margin;
m_min[1] -= margin;
@@ -242,13 +236,11 @@ public:
m_max[2] += margin;
}
GIM_AABB(const GIM_AABB &other):
m_min(other.m_min),m_max(other.m_max)
GIM_AABB(const GIM_AABB &other) : m_min(other.m_min), m_max(other.m_max)
{
}
GIM_AABB(const GIM_AABB &other,btScalar margin ):
m_min(other.m_min),m_max(other.m_max)
GIM_AABB(const GIM_AABB &other, btScalar margin) : m_min(other.m_min), m_max(other.m_max)
{
m_min[0] -= margin;
m_min[1] -= margin;
@@ -289,34 +281,34 @@ public:
m_max[2] = other.m_max[2] + margin;
}
template<typename CLASS_POINT>
template <typename CLASS_POINT>
SIMD_FORCE_INLINE void calc_from_triangle(
const CLASS_POINT & V1,
const CLASS_POINT & V2,
const CLASS_POINT & V3)
const CLASS_POINT &V1,
const CLASS_POINT &V2,
const CLASS_POINT &V3)
{
m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]);
m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]);
m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]);
m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]);
m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]);
m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]);
}
template<typename CLASS_POINT>
template <typename CLASS_POINT>
SIMD_FORCE_INLINE void calc_from_triangle_margin(
const CLASS_POINT & V1,
const CLASS_POINT & V2,
const CLASS_POINT & V3, btScalar margin)
const CLASS_POINT &V1,
const CLASS_POINT &V2,
const CLASS_POINT &V3, btScalar margin)
{
m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
m_min[0] = GIM_MIN3(V1[0], V2[0], V3[0]);
m_min[1] = GIM_MIN3(V1[1], V2[1], V3[1]);
m_min[2] = GIM_MIN3(V1[2], V2[2], V3[2]);
m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
m_max[0] = GIM_MAX3(V1[0], V2[0], V3[0]);
m_max[1] = GIM_MAX3(V1[1], V2[1], V3[1]);
m_max[2] = GIM_MAX3(V1[2], V2[2], V3[2]);
m_min[0] -= margin;
m_min[1] -= margin;
@@ -327,74 +319,73 @@ public:
}
//! Apply a transform to an AABB
SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
SIMD_FORCE_INLINE void appy_transform(const btTransform &trans)
{
btVector3 center = (m_max+m_min)*0.5f;
btVector3 center = (m_max + m_min) * 0.5f;
btVector3 extends = m_max - center;
// Compute new center
center = trans(center);
btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(),
trans.getBasis().getRow(1).absolute(),
trans.getBasis().getRow(2).absolute());
btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(),
trans.getBasis().getRow(1).absolute(),
trans.getBasis().getRow(2).absolute());
m_min = center - textends;
m_max = center + textends;
}
//! Merges a Box
SIMD_FORCE_INLINE void merge(const GIM_AABB & box)
SIMD_FORCE_INLINE void merge(const GIM_AABB &box)
{
m_min[0] = GIM_MIN(m_min[0],box.m_min[0]);
m_min[1] = GIM_MIN(m_min[1],box.m_min[1]);
m_min[2] = GIM_MIN(m_min[2],box.m_min[2]);
m_min[0] = GIM_MIN(m_min[0], box.m_min[0]);
m_min[1] = GIM_MIN(m_min[1], box.m_min[1]);
m_min[2] = GIM_MIN(m_min[2], box.m_min[2]);
m_max[0] = GIM_MAX(m_max[0],box.m_max[0]);
m_max[1] = GIM_MAX(m_max[1],box.m_max[1]);
m_max[2] = GIM_MAX(m_max[2],box.m_max[2]);
m_max[0] = GIM_MAX(m_max[0], box.m_max[0]);
m_max[1] = GIM_MAX(m_max[1], box.m_max[1]);
m_max[2] = GIM_MAX(m_max[2], box.m_max[2]);
}
//! Merges a point
template<typename CLASS_POINT>
SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
template <typename CLASS_POINT>
SIMD_FORCE_INLINE void merge_point(const CLASS_POINT &point)
{
m_min[0] = GIM_MIN(m_min[0],point[0]);
m_min[1] = GIM_MIN(m_min[1],point[1]);
m_min[2] = GIM_MIN(m_min[2],point[2]);
m_min[0] = GIM_MIN(m_min[0], point[0]);
m_min[1] = GIM_MIN(m_min[1], point[1]);
m_min[2] = GIM_MIN(m_min[2], point[2]);
m_max[0] = GIM_MAX(m_max[0],point[0]);
m_max[1] = GIM_MAX(m_max[1],point[1]);
m_max[2] = GIM_MAX(m_max[2],point[2]);
m_max[0] = GIM_MAX(m_max[0], point[0]);
m_max[1] = GIM_MAX(m_max[1], point[1]);
m_max[2] = GIM_MAX(m_max[2], point[2]);
}
//! Gets the extend and center
SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const
SIMD_FORCE_INLINE void get_center_extend(btVector3 &center, btVector3 &extend) const
{
center = (m_max+m_min)*0.5f;
center = (m_max + m_min) * 0.5f;
extend = m_max - center;
}
//! Finds the intersecting box between this box and the other.
SIMD_FORCE_INLINE void find_intersection(const GIM_AABB & other, GIM_AABB & intersection) const
SIMD_FORCE_INLINE void find_intersection(const GIM_AABB &other, GIM_AABB &intersection) const
{
intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]);
intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]);
intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]);
intersection.m_min[0] = GIM_MAX(other.m_min[0], m_min[0]);
intersection.m_min[1] = GIM_MAX(other.m_min[1], m_min[1]);
intersection.m_min[2] = GIM_MAX(other.m_min[2], m_min[2]);
intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]);
intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]);
intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]);
intersection.m_max[0] = GIM_MIN(other.m_max[0], m_max[0]);
intersection.m_max[1] = GIM_MIN(other.m_max[1], m_max[1]);
intersection.m_max[2] = GIM_MIN(other.m_max[2], m_max[2]);
}
SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const
SIMD_FORCE_INLINE bool has_collision(const GIM_AABB &other) const
{
if(m_min[0] > other.m_max[0] ||
m_max[0] < other.m_min[0] ||
m_min[1] > other.m_max[1] ||
m_max[1] < other.m_min[1] ||
m_min[2] > other.m_max[2] ||
m_max[2] < other.m_min[2])
if (m_min[0] > other.m_max[0] ||
m_max[0] < other.m_min[0] ||
m_min[1] > other.m_max[1] ||
m_max[1] < other.m_min[1] ||
m_min[2] > other.m_max[2] ||
m_max[2] < other.m_min[2])
{
return false;
}
@@ -406,35 +397,34 @@ public:
\param vorigin A vec3f with the origin of the ray
\param vdir A vec3f with the direction of the ray
*/
SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)
SIMD_FORCE_INLINE bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir)
{
btVector3 extents,center;
this->get_center_extend(center,extents);;
btVector3 extents, center;
this->get_center_extend(center, extents);
;
btScalar Dx = vorigin[0] - center[0];
if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f) return false;
if (GIM_GREATER(Dx, extents[0]) && Dx * vdir[0] >= 0.0f) return false;
btScalar Dy = vorigin[1] - center[1];
if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f) return false;
if (GIM_GREATER(Dy, extents[1]) && Dy * vdir[1] >= 0.0f) return false;
btScalar Dz = vorigin[2] - center[2];
if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f) return false;
if (GIM_GREATER(Dz, extents[2]) && Dz * vdir[2] >= 0.0f) return false;
btScalar f = vdir[1] * Dz - vdir[2] * Dy;
if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
if (btFabs(f) > extents[1] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[1])) return false;
f = vdir[2] * Dx - vdir[0] * Dz;
if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
if (btFabs(f) > extents[0] * btFabs(vdir[2]) + extents[2] * btFabs(vdir[0])) return false;
f = vdir[0] * Dy - vdir[1] * Dx;
if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
if (btFabs(f) > extents[0] * btFabs(vdir[1]) + extents[1] * btFabs(vdir[0])) return false;
return true;
}
SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
SIMD_FORCE_INLINE void projection_interval(const btVector3 &direction, btScalar &vmin, btScalar &vmax) const
{
btVector3 center = (m_max+m_min)*0.5f;
btVector3 extend = m_max-center;
btVector3 center = (m_max + m_min) * 0.5f;
btVector3 extend = m_max - center;
btScalar _fOrigin = direction.dot(center);
btScalar _fOrigin = direction.dot(center);
btScalar _fMaximumExtent = extend.dot(direction.absolute());
vmin = _fOrigin - _fMaximumExtent;
vmax = _fOrigin + _fMaximumExtent;
@@ -442,22 +432,22 @@ public:
SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
{
btScalar _fmin,_fmax;
this->projection_interval(plane,_fmin,_fmax);
btScalar _fmin, _fmax;
this->projection_interval(plane, _fmin, _fmax);
if(plane[3] > _fmax + BOX_PLANE_EPSILON)
if (plane[3] > _fmax + BOX_PLANE_EPSILON)
{
return G_BACK_PLANE; // 0
return G_BACK_PLANE; // 0
}
if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
if (plane[3] + BOX_PLANE_EPSILON >= _fmin)
{
return G_COLLIDE_PLANE; //1
return G_COLLIDE_PLANE; //1
}
return G_FRONT_PLANE;//2
return G_FRONT_PLANE; //2
}
SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0)
SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB &box, btTransform &trans1_to_0)
{
GIM_AABB tbox = box;
tbox.appy_transform(trans1_to_0);
@@ -466,52 +456,50 @@ public:
//! transcache is the transformation cache from box to this AABB
SIMD_FORCE_INLINE bool overlapping_trans_cache(
const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest)
const GIM_AABB &box, const GIM_BOX_BOX_TRANSFORM_CACHE &transcache, bool fulltest)
{
//Taken from OPCODE
btVector3 ea,eb;//extends
btVector3 ca,cb;//extends
get_center_extend(ca,ea);
box.get_center_extend(cb,eb);
btVector3 ea, eb; //extends
btVector3 ca, cb; //extends
get_center_extend(ca, ea);
box.get_center_extend(cb, eb);
btVector3 T;
btScalar t,t2;
btScalar t, t2;
int i;
// Class I : A's basis vectors
for(i=0;i<3;i++)
for (i = 0; i < 3; i++)
{
T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
T[i] = transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
t = transcache.m_AR[i].dot(eb) + ea[i];
if(GIM_GREATER(T[i], t)) return false;
if (GIM_GREATER(T[i], t)) return false;
}
// Class II : B's basis vectors
for(i=0;i<3;i++)
for (i = 0; i < 3; i++)
{
t = MAT_DOT_COL(transcache.m_R1to0,T,i);
t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i];
if(GIM_GREATER(t,t2)) return false;
t = MAT_DOT_COL(transcache.m_R1to0, T, i);
t2 = MAT_DOT_COL(transcache.m_AR, ea, i) + eb[i];
if (GIM_GREATER(t, t2)) return false;
}
// Class III : 9 cross products
if(fulltest)
if (fulltest)
{
int j,m,n,o,p,q,r;
for(i=0;i<3;i++)
int j, m, n, o, p, q, r;
for (i = 0; i < 3; i++)
{
m = (i+1)%3;
n = (i+2)%3;
o = i==0?1:0;
p = i==2?1:2;
for(j=0;j<3;j++)
m = (i + 1) % 3;
n = (i + 2) % 3;
o = i == 0 ? 1 : 0;
p = i == 2 ? 1 : 2;
for (j = 0; j < 3; j++)
{
q = j==2?1:2;
r = j==0?1:0;
t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
if(GIM_GREATER(t,t2)) return false;
q = j == 2 ? 1 : 2;
r = j == 0 ? 1 : 0;
t = T[n] * transcache.m_R1to0[m][j] - T[m] * transcache.m_R1to0[n][j];
t2 = ea[o] * transcache.m_AR[p][j] + ea[p] * transcache.m_AR[o][j] +
eb[r] * transcache.m_AR[i][q] + eb[q] * transcache.m_AR[i][r];
if (GIM_GREATER(t, t2)) return false;
}
}
}
@@ -520,7 +508,7 @@ public:
//! Simple test for planes.
SIMD_FORCE_INLINE bool collide_plane(
const btVector4 & plane)
const btVector4 &plane)
{
ePLANE_INTERSECTION_TYPE classify = plane_classify(plane);
return (classify == G_COLLIDE_PLANE);
@@ -528,15 +516,15 @@ public:
//! test for a triangle, with edges
SIMD_FORCE_INLINE bool collide_triangle_exact(
const btVector3 & p1,
const btVector3 & p2,
const btVector3 & p3,
const btVector4 & triangle_plane)
const btVector3 &p1,
const btVector3 &p2,
const btVector3 &p3,
const btVector4 &triangle_plane)
{
if(!collide_plane(triangle_plane)) return false;
if (!collide_plane(triangle_plane)) return false;
btVector3 center,extends;
this->get_center_extend(center,extends);
btVector3 center, extends;
this->get_center_extend(center, extends);
const btVector3 v1(p1 - center);
const btVector3 v2(p2 - center);
@@ -546,30 +534,29 @@ public:
btVector3 diff(v2 - v1);
btVector3 abs_diff = diff.absolute();
//Test With X axis
TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v1, v3, extends);
//Test With Y axis
TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v1, v3, extends);
//Test With Z axis
TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v1, v3, extends);
diff = v3 - v2;
abs_diff = diff.absolute();
//Test With X axis
TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v2, v1, extends);
//Test With Y axis
TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v2, v1, extends);
//Test With Z axis
TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v2, v1, extends);
diff = v1 - v3;
abs_diff = diff.absolute();
//Test With X axis
TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff, abs_diff, v3, v2, extends);
//Test With Y axis
TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff, abs_diff, v3, v2, extends);
//Test With Z axis
TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff, abs_diff, v3, v2, extends);
return true;
}
@@ -577,17 +564,15 @@ public:
#ifndef BT_BOX_COLLISION_H_INCLUDED
//! Compairison of transformation objects
SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform &t1, const btTransform &t2)
{
if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
if (!(t1.getOrigin() == t2.getOrigin())) return false;
if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
if (!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0))) return false;
if (!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1))) return false;
if (!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2))) return false;
return true;
}
#endif
#endif // GIM_BOX_COLLISION_H_INCLUDED
#endif // GIM_BOX_COLLISION_H_INCLUDED

88
src/BulletCollision/Gimpact/gim_box_set.cpp
View File

@@ -28,67 +28,64 @@ email: projectileman@yahoo.com
-----------------------------------------------------------------------------
*/
#include "gim_box_set.h"
GUINT GIM_BOX_TREE::_calc_splitting_axis(
gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex)
gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex)
{
GUINT i;
btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
GUINT numIndices = endIndex-startIndex;
btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.));
GUINT numIndices = endIndex - startIndex;
for (i=startIndex;i<endIndex;i++)
for (i = startIndex; i < endIndex; i++)
{
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
means+=center;
btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
means += center;
}
means *= (btScalar(1.)/(btScalar)numIndices);
means *= (btScalar(1.) / (btScalar)numIndices);
for (i=startIndex;i<endIndex;i++)
for (i = startIndex; i < endIndex; i++)
{
btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
btVector3 diff2 = center-means;
btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
primitive_boxes[i].m_bound.m_min);
btVector3 diff2 = center - means;
diff2 = diff2 * diff2;
variance += diff2;
}
variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );
variance *= (btScalar(1.) / ((btScalar)numIndices - 1));
return variance.maxAxis();
}
GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index(
gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex,
gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex,
GUINT endIndex, GUINT splitAxis)
{
GUINT i;
GUINT splitIndex =startIndex;
GUINT splitIndex = startIndex;
GUINT numIndices = endIndex - startIndex;
// average of centers
btScalar splitValue = 0.0f;
for (i=startIndex;i<endIndex;i++)
for (i = startIndex; i < endIndex; i++)
{
splitValue+= 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
primitive_boxes[i].m_bound.m_min[splitAxis]);
splitValue += 0.5f * (primitive_boxes[i].m_bound.m_max[splitAxis] +
primitive_boxes[i].m_bound.m_min[splitAxis]);
}
splitValue /= (btScalar)numIndices;
//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
for (i=startIndex;i<endIndex;i++)
for (i = startIndex; i < endIndex; i++)
{
btScalar center = 0.5f*(primitive_boxes[i].m_bound.m_max[splitAxis] +
primitive_boxes[i].m_bound.m_min[splitAxis]);
btScalar center = 0.5f * (primitive_boxes[i].m_bound.m_max[splitAxis] +
primitive_boxes[i].m_bound.m_min[splitAxis]);
if (center > splitValue)
{
//swap
primitive_boxes.swap(i,splitIndex);
primitive_boxes.swap(i, splitIndex);
splitIndex++;
}
}
@@ -102,28 +99,27 @@ GUINT GIM_BOX_TREE::_sort_and_calc_splitting_index(
//bool unbalanced2 = true;
//this should be safe too:
GUINT rangeBalancedIndices = numIndices/3;
bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
GUINT rangeBalancedIndices = numIndices / 3;
bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
if (unbalanced)
{
splitIndex = startIndex+ (numIndices>>1);
splitIndex = startIndex + (numIndices >> 1);
}
btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex))));
return splitIndex;
}
void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex)
void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex)
{
GUINT current_index = m_num_nodes++;
btAssert((endIndex-startIndex)>0);
btAssert((endIndex - startIndex) > 0);
if((endIndex-startIndex) == 1) //we got a leaf
{
if ((endIndex - startIndex) == 1) //we got a leaf
{
m_node_array[current_index].m_left = 0;
m_node_array[current_index].m_right = 0;
m_node_array[current_index].m_escapeIndex = 0;
@@ -138,8 +134,8 @@ void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, G
GUINT splitIndex;
//calc this node bounding box
m_node_array[current_index].m_bound.invalidate();
for (splitIndex=startIndex;splitIndex<endIndex;splitIndex++)
m_node_array[current_index].m_bound.invalidate();
for (splitIndex = startIndex; splitIndex < endIndex; splitIndex++)
{
m_node_array[current_index].m_bound.merge(primitive_boxes[splitIndex].m_bound);
}
@@ -147,36 +143,34 @@ void GIM_BOX_TREE::_build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, G
//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
//split axis
splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex);
splitIndex = _sort_and_calc_splitting_index(
primitive_boxes,startIndex,endIndex,splitIndex);
primitive_boxes, startIndex, endIndex, splitIndex);
//configure this inner node : the left node index
m_node_array[current_index].m_left = m_num_nodes;
//build left child tree
_build_sub_tree(primitive_boxes, startIndex, splitIndex );
_build_sub_tree(primitive_boxes, startIndex, splitIndex);
//configure this inner node : the right node index
m_node_array[current_index].m_right = m_num_nodes;
//build right child tree
_build_sub_tree(primitive_boxes, splitIndex ,endIndex);
_build_sub_tree(primitive_boxes, splitIndex, endIndex);
//configure this inner node : the escape index
m_node_array[current_index].m_escapeIndex = m_num_nodes - current_index;
m_node_array[current_index].m_escapeIndex = m_num_nodes - current_index;
}
//! stackless build tree
void GIM_BOX_TREE::build_tree(
gim_array<GIM_AABB_DATA> & primitive_boxes)
gim_array<GIM_AABB_DATA>& primitive_boxes)
{
// initialize node count to 0
m_num_nodes = 0;
// allocate nodes
m_node_array.resize(primitive_boxes.size()*2);
m_node_array.resize(primitive_boxes.size() * 2);
_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
}

275
src/BulletCollision/Gimpact/gim_box_set.h
View File

@@ -33,54 +33,51 @@ email: projectileman@yahoo.com
-----------------------------------------------------------------------------
*/
#include "gim_array.h"
#include "gim_radixsort.h"
#include "gim_box_collision.h"
#include "gim_tri_collision.h"
//! Overlapping pair
struct GIM_PAIR
{
GUINT m_index1;
GUINT m_index2;
GIM_PAIR()
{}
GUINT m_index1;
GUINT m_index2;
GIM_PAIR()
{
}
GIM_PAIR(const GIM_PAIR & p)
{
m_index1 = p.m_index1;
m_index2 = p.m_index2;
GIM_PAIR(const GIM_PAIR& p)
{
m_index1 = p.m_index1;
m_index2 = p.m_index2;
}
GIM_PAIR(GUINT index1, GUINT index2)
{
m_index1 = index1;
m_index2 = index2;
{
m_index1 = index1;
m_index2 = index2;
}
};
//! A pairset array
class gim_pair_set: public gim_array<GIM_PAIR>
class gim_pair_set : public gim_array<GIM_PAIR>
{
public:
gim_pair_set():gim_array<GIM_PAIR>(32)
gim_pair_set() : gim_array<GIM_PAIR>(32)
{
}
inline void push_pair(GUINT index1,GUINT index2)
inline void push_pair(GUINT index1, GUINT index2)
{
push_back(GIM_PAIR(index1,index2));
push_back(GIM_PAIR(index1, index2));
}
inline void push_pair_inv(GUINT index1,GUINT index2)
inline void push_pair_inv(GUINT index1, GUINT index2)
{
push_back(GIM_PAIR(index2,index1));
push_back(GIM_PAIR(index2, index1));
}
};
//! Prototype Base class for primitive classification
/*!
This class is a wrapper for primitive collections.
@@ -90,16 +87,14 @@ This class can manage Compound shapes and trimeshes, and if it is managing trime
class GIM_PRIMITIVE_MANAGER_PROTOTYPE
{
public:
virtual ~GIM_PRIMITIVE_MANAGER_PROTOTYPE() {}
//! determines if this manager consist on only triangles, which special case will be optimized
virtual bool is_trimesh() = 0;
virtual GUINT get_primitive_count() = 0;
virtual void get_primitive_box(GUINT prim_index ,GIM_AABB & primbox) = 0;
virtual void get_primitive_triangle(GUINT prim_index,GIM_TRIANGLE & triangle) = 0;
virtual void get_primitive_box(GUINT prim_index, GIM_AABB& primbox) = 0;
virtual void get_primitive_triangle(GUINT prim_index, GIM_TRIANGLE& triangle) = 0;
};
struct GIM_AABB_DATA
{
GIM_AABB m_bound;
@@ -110,22 +105,22 @@ struct GIM_AABB_DATA
struct GIM_BOX_TREE_NODE
{
GIM_AABB m_bound;
GUINT m_left;//!< Left subtree
GUINT m_right;//!< Right subtree
GUINT m_escapeIndex;//!< Scape index for traversing
GUINT m_data;//!< primitive index if apply
GUINT m_left; //!< Left subtree
GUINT m_right; //!< Right subtree
GUINT m_escapeIndex; //!< Scape index for traversing
GUINT m_data; //!< primitive index if apply
GIM_BOX_TREE_NODE()
{
m_left = 0;
m_right = 0;
m_escapeIndex = 0;
m_data = 0;
m_left = 0;
m_right = 0;
m_escapeIndex = 0;
m_data = 0;
}
SIMD_FORCE_INLINE bool is_leaf_node() const
{
return (!m_left && !m_right);
return (!m_left && !m_right);
}
};
@@ -135,14 +130,16 @@ class GIM_BOX_TREE
protected:
GUINT m_num_nodes;
gim_array<GIM_BOX_TREE_NODE> m_node_array;
protected:
GUINT _sort_and_calc_splitting_index(
gim_array<GIM_AABB_DATA> & primitive_boxes,
GUINT startIndex, GUINT endIndex, GUINT splitAxis);
gim_array<GIM_AABB_DATA>& primitive_boxes,
GUINT startIndex, GUINT endIndex, GUINT splitAxis);
GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex);
GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex);
void _build_sub_tree(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex);
void _build_sub_tree(gim_array<GIM_AABB_DATA> & primitive_boxes, GUINT startIndex, GUINT endIndex);
public:
GIM_BOX_TREE()
{
@@ -151,7 +148,7 @@ public:
//! prototype functions for box tree management
//!@{
void build_tree(gim_array<GIM_AABB_DATA> & primitive_boxes);
void build_tree(gim_array<GIM_AABB_DATA>& primitive_boxes);
SIMD_FORCE_INLINE void clearNodes()
{
@@ -176,22 +173,22 @@ public:
return m_node_array[nodeindex].m_data;
}
SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const
SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const
{
bound = m_node_array[nodeindex].m_bound;
}
SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound)
{
m_node_array[nodeindex].m_bound = bound;
}
SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const
SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const
{
return m_node_array[nodeindex].m_left;
}
SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const
SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const
{
return m_node_array[nodeindex].m_right;
}
@@ -204,78 +201,78 @@ public:
//!@}
};
//! Generic Box Tree Template
/*!
This class offers an structure for managing a box tree of primitives.
Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and
a Box tree structure ( like GIM_BOX_TREE).
*/
template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE>
template <typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE>
class GIM_BOX_TREE_TEMPLATE_SET
{
protected:
_GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager;
_GIM_BOX_TREE_PROTOTYPE m_box_tree;
protected:
//stackless refit
SIMD_FORCE_INLINE void refit()
{
GUINT nodecount = getNodeCount();
while(nodecount--)
while (nodecount--)
{
if(isLeafNode(nodecount))
if (isLeafNode(nodecount))
{
GIM_AABB leafbox;
m_primitive_manager.get_primitive_box(getNodeData(nodecount),leafbox);
setNodeBound(nodecount,leafbox);
m_primitive_manager.get_primitive_box(getNodeData(nodecount), leafbox);
setNodeBound(nodecount, leafbox);
}
else
{
//get left bound
GUINT childindex = getLeftNodeIndex(nodecount);
GIM_AABB bound;
getNodeBound(childindex,bound);
getNodeBound(childindex, bound);
//get right bound
childindex = getRightNodeIndex(nodecount);
GIM_AABB bound2;
getNodeBound(childindex,bound2);
getNodeBound(childindex, bound2);
bound.merge(bound2);
setNodeBound(nodecount,bound);
setNodeBound(nodecount, bound);
}
}
}
public:
public:
GIM_BOX_TREE_TEMPLATE_SET()
{
}
SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const
SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const
{
GIM_AABB totalbox;
getNodeBound(0, totalbox);
return totalbox;
}
SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & primitive_manager)
SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& primitive_manager)
{
m_primitive_manager = primitive_manager;
}
const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const
const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() const
{
return m_primitive_manager;
}
_GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager()
_GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager()
{
return m_primitive_manager;
}
//! node manager prototype functions
///@{
//! node manager prototype functions
///@{
//! this attemps to refit the box set.
SIMD_FORCE_INLINE void update()
@@ -288,19 +285,19 @@ public:
{
//obtain primitive boxes
gim_array<GIM_AABB_DATA> primitive_boxes;
primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false);
primitive_boxes.resize(m_primitive_manager.get_primitive_count(), false);
for (GUINT i = 0;i<primitive_boxes.size() ;i++ )
for (GUINT i = 0; i < primitive_boxes.size(); i++)
{
m_primitive_manager.get_primitive_box(i,primitive_boxes[i].m_bound);
primitive_boxes[i].m_data = i;
m_primitive_manager.get_primitive_box(i, primitive_boxes[i].m_bound);
primitive_boxes[i].m_data = i;
}
m_box_tree.build_tree(primitive_boxes);
}
//! returns the indices of the primitives in the m_primitive_manager
SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB & box, gim_array<GUINT> & collided_results) const
SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB& box, gim_array<GUINT>& collided_results) const
{
GUINT curIndex = 0;
GUINT numNodes = getNodeCount();
@@ -308,7 +305,7 @@ public:
while (curIndex < numNodes)
{
GIM_AABB bound;
getNodeBound(curIndex,bound);
getNodeBound(curIndex, bound);
//catch bugs in tree data
@@ -328,26 +325,26 @@ public:
else
{
//skip node
curIndex+= getScapeNodeIndex(curIndex);
curIndex += getScapeNodeIndex(curIndex);
}
}
if(collided_results.size()>0) return true;
if (collided_results.size() > 0) return true;
return false;
}
//! returns the indices of the primitives in the m_primitive_manager
SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB & box,
const btTransform & transform, gim_array<GUINT> & collided_results) const
SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB& box,
const btTransform& transform, gim_array<GUINT>& collided_results) const
{
GIM_AABB transbox=box;
GIM_AABB transbox = box;
transbox.appy_transform(transform);
return boxQuery(transbox,collided_results);
return boxQuery(transbox, collided_results);
}
//! returns the indices of the primitives in the m_primitive_manager
SIMD_FORCE_INLINE bool rayQuery(
const btVector3 & ray_dir,const btVector3 & ray_origin ,
gim_array<GUINT> & collided_results) const
const btVector3& ray_dir, const btVector3& ray_origin,
gim_array<GUINT>& collided_results) const
{
GUINT curIndex = 0;
GUINT numNodes = getNodeCount();
@@ -355,16 +352,16 @@ public:
while (curIndex < numNodes)
{
GIM_AABB bound;
getNodeBound(curIndex,bound);
getNodeBound(curIndex, bound);
//catch bugs in tree data
bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir);
bool isleafnode = isLeafNode(curIndex);
if (isleafnode && aabbOverlap)
{
collided_results.push_back(getNodeData( curIndex));
collided_results.push_back(getNodeData(curIndex));
}
if (aabbOverlap || isleafnode)
@@ -375,10 +372,10 @@ public:
else
{
//skip node
curIndex+= getScapeNodeIndex(curIndex);
curIndex += getScapeNodeIndex(curIndex);
}
}
if(collided_results.size()>0) return true;
if (collided_results.size() > 0) return true;
return false;
}
@@ -389,7 +386,7 @@ public:
}
//! tells if this set is a trimesh
SIMD_FORCE_INLINE bool isTrimesh() const
SIMD_FORCE_INLINE bool isTrimesh() const
{
return m_primitive_manager.is_trimesh();
}
@@ -411,12 +408,12 @@ public:
return m_box_tree.getNodeData(nodeindex);
}
SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB & bound) const
SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const
{
m_box_tree.getNodeBound(nodeindex, bound);
}
SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB & bound)
SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound)
{
m_box_tree.setNodeBound(nodeindex, bound);
}
@@ -436,36 +433,30 @@ public:
return m_box_tree.getScapeNodeIndex(nodeindex);
}
SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const
SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex, GIM_TRIANGLE& triangle) const
{
m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex),triangle);
m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex), triangle);
}
};
//! Class for Box Tree Sets
/*!
this has the GIM_BOX_TREE implementation for bounding boxes.
*/
template<typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE>
class GIM_BOX_TREE_SET: public GIM_BOX_TREE_TEMPLATE_SET< _GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE>
template <typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE>
class GIM_BOX_TREE_SET : public GIM_BOX_TREE_TEMPLATE_SET<_GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE>
{
public:
};
/// GIM_BOX_SET collision methods
template<typename BOX_SET_CLASS0,typename BOX_SET_CLASS1>
template <typename BOX_SET_CLASS0, typename BOX_SET_CLASS1>
class GIM_TREE_TREE_COLLIDER
{
public:
gim_pair_set * m_collision_pairs;
BOX_SET_CLASS0 * m_boxset0;
BOX_SET_CLASS1 * m_boxset1;
gim_pair_set* m_collision_pairs;
BOX_SET_CLASS0* m_boxset0;
BOX_SET_CLASS1* m_boxset1;
GUINT current_node0;
GUINT current_node1;
bool node0_is_leaf;
@@ -483,18 +474,18 @@ public:
GIM_TRIANGLE m_tri1;
btVector4 m_tri1_plane;
public:
GIM_TREE_TREE_COLLIDER()
{
current_node0 = G_UINT_INFINITY;
current_node1 = G_UINT_INFINITY;
}
protected:
SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0)
{
if(node0_has_triangle) return;
m_boxset0->getNodeTriangle(node0,m_tri0);
if (node0_has_triangle) return;
m_boxset0->getNodeTriangle(node0, m_tri0);
//transform triangle
m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]);
m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]);
@@ -506,8 +497,8 @@ protected:
SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1)
{
if(node1_has_triangle) return;
m_boxset1->getNodeTriangle(node1,m_tri1);
if (node1_has_triangle) return;
m_boxset1->getNodeTriangle(node1, m_tri1);
//transform triangle
m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]);
m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]);
@@ -519,8 +510,8 @@ protected:
SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0)
{
if(node0 == current_node0) return;
m_boxset0->getNodeBound(node0,m_box0);
if (node0 == current_node0) return;
m_boxset0->getNodeBound(node0, m_box0);
node0_is_leaf = m_boxset0->isLeafNode(node0);
node0_has_triangle = false;
current_node0 = node0;
@@ -528,21 +519,21 @@ protected:
SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1)
{
if(node1 == current_node1) return;
m_boxset1->getNodeBound(node1,m_box1);
if (node1 == current_node1) return;
m_boxset1->getNodeBound(node1, m_box1);
node1_is_leaf = m_boxset1->isLeafNode(node1);
node1_has_triangle = false;
current_node1 = node1;
}
SIMD_FORCE_INLINE bool node_collision(GUINT node0 ,GUINT node1)
SIMD_FORCE_INLINE bool node_collision(GUINT node0, GUINT node1)
{
retrieve_node0_info(node0);
retrieve_node1_info(node1);
bool result = m_box0.overlapping_trans_cache(m_box1,trans_cache_1to0,true);
if(!result) return false;
bool result = m_box0.overlapping_trans_cache(m_box1, trans_cache_1to0, true);
if (!result) return false;
if(t0_is_trimesh && node0_is_leaf)
if (t0_is_trimesh && node0_is_leaf)
{
//perform primitive vs box collision
retrieve_node0_triangle(node0);
@@ -550,14 +541,14 @@ protected:
m_box1.increment_margin(m_tri0.m_margin);
result = m_box1.collide_triangle_exact(
m_tri0.m_vertices[0],m_tri0.m_vertices[1],m_tri0.m_vertices[2],m_tri0_plane);
m_tri0.m_vertices[0], m_tri0.m_vertices[1], m_tri0.m_vertices[2], m_tri0_plane);
m_box1.increment_margin(-m_tri0.m_margin);
if(!result) return false;
if (!result) return false;
return true;
}
else if(t1_is_trimesh && node1_is_leaf)
else if (t1_is_trimesh && node1_is_leaf)
{
//perform primitive vs box collision
retrieve_node1_triangle(node1);
@@ -565,11 +556,11 @@ protected:
m_box0.increment_margin(m_tri1.m_margin);
result = m_box0.collide_triangle_exact(
m_tri1.m_vertices[0],m_tri1.m_vertices[1],m_tri1.m_vertices[2],m_tri1_plane);
m_tri1.m_vertices[0], m_tri1.m_vertices[1], m_tri1.m_vertices[2], m_tri1_plane);
m_box0.increment_margin(-m_tri1.m_margin);
if(!result) return false;
if (!result) return false;
return true;
}
return true;
@@ -582,40 +573,39 @@ protected:
stack_collisions.reserve(32);
//add the first pair
stack_collisions.push_pair(0,0);
stack_collisions.push_pair(0, 0);
while(stack_collisions.size())
while (stack_collisions.size())
{
//retrieve the last pair and pop
GUINT node0 = stack_collisions.back().m_index1;
GUINT node1 = stack_collisions.back().m_index2;
stack_collisions.pop_back();
if(node_collision(node0,node1)) // a collision is found
if (node_collision(node0, node1)) // a collision is found
{
if(node0_is_leaf)
if (node0_is_leaf)
{
if(node1_is_leaf)
if (node1_is_leaf)
{
m_collision_pairs->push_pair(m_boxset0->getNodeData(node0),m_boxset1->getNodeData(node1));
m_collision_pairs->push_pair(m_boxset0->getNodeData(node0), m_boxset1->getNodeData(node1));
}
else
{
//collide left
stack_collisions.push_pair(node0,m_boxset1->getLeftNodeIndex(node1));
stack_collisions.push_pair(node0, m_boxset1->getLeftNodeIndex(node1));
//collide right
stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1));
stack_collisions.push_pair(node0, m_boxset1->getRightNodeIndex(node1));
}
}
else
{
if(node1_is_leaf)
if (node1_is_leaf)
{
//collide left
stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1);
stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0), node1);
//collide right
stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1);
stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0), node1);
}
else
{
@@ -624,36 +614,36 @@ protected:
GUINT left1 = m_boxset1->getLeftNodeIndex(node1);
GUINT right1 = m_boxset1->getRightNodeIndex(node1);
//collide left
stack_collisions.push_pair(left0,left1);
stack_collisions.push_pair(left0, left1);
//collide right
stack_collisions.push_pair(left0,right1);
stack_collisions.push_pair(left0, right1);
//collide left
stack_collisions.push_pair(right0,left1);
stack_collisions.push_pair(right0, left1);
//collide right
stack_collisions.push_pair(right0,right1);
stack_collisions.push_pair(right0, right1);
}// else if node1 is not a leaf
}// else if node0 is not a leaf
} // else if node1 is not a leaf
} // else if node0 is not a leaf
}// if(node_collision(node0,node1))
}//while(stack_collisions.size())
} // if(node_collision(node0,node1))
} //while(stack_collisions.size())
}
public:
void find_collision(BOX_SET_CLASS0 * boxset1, const btTransform & trans1,
BOX_SET_CLASS1 * boxset2, const btTransform & trans2,
gim_pair_set & collision_pairs, bool complete_primitive_tests = true)
void find_collision(BOX_SET_CLASS0* boxset1, const btTransform& trans1,
BOX_SET_CLASS1* boxset2, const btTransform& trans2,
gim_pair_set& collision_pairs, bool complete_primitive_tests = true)
{
m_collision_pairs = &collision_pairs;
m_boxset0 = boxset1;
m_boxset1 = boxset2;
trans_cache_1to0.calc_from_homogenic(trans1,trans2);
trans_cache_1to0.calc_from_homogenic(trans1, trans2);
trans_cache_0to1 = trans2.inverse();
trans_cache_0to1 = trans2.inverse();
trans_cache_0to1 *= trans1;
if(complete_primitive_tests)
if (complete_primitive_tests)
{
t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh();
t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh();
@@ -668,7 +658,4 @@ public:
}
};
#endif // GIM_BOXPRUNING_H_INCLUDED
#endif // GIM_BOXPRUNING_H_INCLUDED

181
src/BulletCollision/Gimpact/gim_clip_polygon.h
View File

@@ -33,91 +33,86 @@ email: projectileman@yahoo.com
-----------------------------------------------------------------------------
*/
//! This function calcs the distance from a 3D plane
class DISTANCE_PLANE_3D_FUNC
{
public:
template<typename CLASS_POINT,typename CLASS_PLANE>
inline GREAL operator()(const CLASS_PLANE & plane, const CLASS_POINT & point)
template <typename CLASS_POINT, typename CLASS_PLANE>
inline GREAL operator()(const CLASS_PLANE& plane, const CLASS_POINT& point)
{
return DISTANCE_PLANE_POINT(plane, point);
}
};
template<typename CLASS_POINT>
template <typename CLASS_POINT>
SIMD_FORCE_INLINE void PLANE_CLIP_POLYGON_COLLECT(
const CLASS_POINT & point0,
const CLASS_POINT & point1,
GREAL dist0,
GREAL dist1,
CLASS_POINT * clipped,
GUINT & clipped_count)
const CLASS_POINT& point0,
const CLASS_POINT& point1,
GREAL dist0,
GREAL dist1,
CLASS_POINT* clipped,
GUINT& clipped_count)
{
GUINT _prevclassif = (dist0>G_EPSILON);
GUINT _classif = (dist1>G_EPSILON);
if(_classif!=_prevclassif)
GUINT _prevclassif = (dist0 > G_EPSILON);
GUINT _classif = (dist1 > G_EPSILON);
if (_classif != _prevclassif)
{
GREAL blendfactor = -dist0/(dist1-dist0);
VEC_BLEND(clipped[clipped_count],point0,point1,blendfactor);
GREAL blendfactor = -dist0 / (dist1 - dist0);
VEC_BLEND(clipped[clipped_count], point0, point1, blendfactor);
clipped_count++;
}
if(!_classif)
if (!_classif)
{
VEC_COPY(clipped[clipped_count],point1);
VEC_COPY(clipped[clipped_count], point1);
clipped_count++;
}
}
//! Clips a polygon by a plane
/*!
*\return The count of the clipped counts
*/
template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
template <typename CLASS_POINT, typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC(
const CLASS_PLANE & plane,
const CLASS_POINT * polygon_points,
GUINT polygon_point_count,
CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
const CLASS_PLANE& plane,
const CLASS_POINT* polygon_points,
GUINT polygon_point_count,
CLASS_POINT* clipped, DISTANCE_PLANE_FUNC distance_func)
{
GUINT clipped_count = 0;
GUINT clipped_count = 0;
//clip first point
GREAL firstdist = distance_func(plane,polygon_points[0]);;
if(!(firstdist>G_EPSILON))
//clip first point
GREAL firstdist = distance_func(plane, polygon_points[0]);
;
if (!(firstdist > G_EPSILON))
{
VEC_COPY(clipped[clipped_count],polygon_points[0]);
VEC_COPY(clipped[clipped_count], polygon_points[0]);
clipped_count++;
}
GREAL olddist = firstdist;
for(GUINT _i=1;_i<polygon_point_count;_i++)
{
GREAL dist = distance_func(plane,polygon_points[_i]);
for (GUINT _i = 1; _i < polygon_point_count; _i++)
{
GREAL dist = distance_func(plane, polygon_points[_i]);
PLANE_CLIP_POLYGON_COLLECT(
polygon_points[_i-1],polygon_points[_i],
olddist,
dist,
clipped,
clipped_count);
polygon_points[_i - 1], polygon_points[_i],
olddist,
dist,
clipped,
clipped_count);
olddist = dist;
olddist = dist;
}
//RETURN TO FIRST point
//RETURN TO FIRST point
PLANE_CLIP_POLYGON_COLLECT(
polygon_points[polygon_point_count-1],polygon_points[0],
olddist,
firstdist,
clipped,
clipped_count);
polygon_points[polygon_point_count - 1], polygon_points[0],
olddist,
firstdist,
clipped,
clipped_count);
return clipped_count;
}
@@ -126,85 +121,79 @@ SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON_GENERIC(
/*!
*\return The count of the clipped counts
*/
template<typename CLASS_POINT,typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
template <typename CLASS_POINT, typename CLASS_PLANE, typename DISTANCE_PLANE_FUNC>
SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE_GENERIC(
const CLASS_PLANE & plane,
const CLASS_POINT & point0,
const CLASS_POINT & point1,
const CLASS_POINT & point2,
CLASS_POINT * clipped,DISTANCE_PLANE_FUNC distance_func)
const CLASS_PLANE& plane,
const CLASS_POINT& point0,
const CLASS_POINT& point1,
const CLASS_POINT& point2,
CLASS_POINT* clipped, DISTANCE_PLANE_FUNC distance_func)
{
GUINT clipped_count = 0;
GUINT clipped_count = 0;
//clip first point
GREAL firstdist = distance_func(plane,point0);;
if(!(firstdist>G_EPSILON))
//clip first point
GREAL firstdist = distance_func(plane, point0);
;
if (!(firstdist > G_EPSILON))
{
VEC_COPY(clipped[clipped_count],point0);
VEC_COPY(clipped[clipped_count], point0);
clipped_count++;
}
// point 1
GREAL olddist = firstdist;
GREAL dist = distance_func(plane,point1);
GREAL dist = distance_func(plane, point1);
PLANE_CLIP_POLYGON_COLLECT(
point0,point1,
olddist,
dist,
clipped,
clipped_count);
point0, point1,
olddist,
dist,
clipped,
clipped_count);
olddist = dist;
// point 2
dist = distance_func(plane,point2);
dist = distance_func(plane, point2);
PLANE_CLIP_POLYGON_COLLECT(
point1,point2,
olddist,
dist,
clipped,
clipped_count);
point1, point2,
olddist,
dist,
clipped,
clipped_count);
olddist = dist;
//RETURN TO FIRST point
PLANE_CLIP_POLYGON_COLLECT(
point2,point0,
olddist,
firstdist,
clipped,
clipped_count);
point2, point0,
olddist,
firstdist,
clipped,
clipped_count);
return clipped_count;
}
template<typename CLASS_POINT,typename CLASS_PLANE>
template <typename CLASS_POINT, typename CLASS_PLANE>
SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON3D(
const CLASS_PLANE & plane,
const CLASS_POINT * polygon_points,
GUINT polygon_point_count,
CLASS_POINT * clipped)
const CLASS_PLANE& plane,
const CLASS_POINT* polygon_points,
GUINT polygon_point_count,
CLASS_POINT* clipped)
{
return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,polygon_points,polygon_point_count,clipped,DISTANCE_PLANE_3D_FUNC());
return PLANE_CLIP_POLYGON_GENERIC<CLASS_POINT, CLASS_PLANE>(plane, polygon_points, polygon_point_count, clipped, DISTANCE_PLANE_3D_FUNC());
}
template<typename CLASS_POINT,typename CLASS_PLANE>
template <typename CLASS_POINT, typename CLASS_PLANE>
SIMD_FORCE_INLINE GUINT PLANE_CLIP_TRIANGLE3D(
const CLASS_PLANE & plane,
const CLASS_POINT & point0,
const CLASS_POINT & point1,
const CLASS_POINT & point2,
CLASS_POINT * clipped)
const CLASS_PLANE& plane,
const CLASS_POINT& point0,
const CLASS_POINT& point1,
const CLASS_POINT& point2,
CLASS_POINT* clipped)
{
return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT,CLASS_PLANE>(plane,point0,point1,point2,clipped,DISTANCE_PLANE_3D_FUNC());
return PLANE_CLIP_TRIANGLE_GENERIC<CLASS_POINT, CLASS_PLANE>(plane, point0, point1, point2, clipped, DISTANCE_PLANE_3D_FUNC());
}
#endif // GIM_TRI_COLLISION_H_INCLUDED
#endif // GIM_TRI_COLLISION_H_INCLUDED

74
src/BulletCollision/Gimpact/gim_contact.cpp
View File

@@ -33,91 +33,89 @@ email: projectileman@yahoo.com
#define MAX_COINCIDENT 8
void gim_contact_array::merge_contacts(
const gim_contact_array & contacts, bool normal_contact_average)
const gim_contact_array& contacts, bool normal_contact_average)
{
clear();
if(contacts.size()==1)
if (contacts.size() == 1)
{
push_back(contacts.back());
return;
}
gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size());
keycontacts.resize(contacts.size(),false);
keycontacts.resize(contacts.size(), false);
//fill key contacts
GUINT i;
for (i = 0;i<contacts.size() ;i++ )
for (i = 0; i < contacts.size(); i++)
{
keycontacts[i].m_key = contacts[i].calc_key_contact();
keycontacts[i].m_value = i;
}
//sort keys
gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR());
gim_heap_sort(keycontacts.pointer(), keycontacts.size(), GIM_RSORT_TOKEN_COMPARATOR());
// Merge contacts
GUINT coincident_count=0;
GUINT coincident_count = 0;
btVector3 coincident_normals[MAX_COINCIDENT];
GUINT last_key = keycontacts[0].m_key;
GUINT key = 0;
push_back(contacts[keycontacts[0].m_value]);
GIM_CONTACT * pcontact = &back();
GIM_CONTACT* pcontact = &back();
for( i=1;i<keycontacts.size();i++)
for (i = 1; i < keycontacts.size(); i++)
{
key = keycontacts[i].m_key;
const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];
key = keycontacts[i].m_key;
const GIM_CONTACT* scontact = &contacts[keycontacts[i].m_value];
if(last_key == key)//same points
if (last_key == key) //same points
{
//merge contact
if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
if (pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth) //)
{
*pcontact = *scontact;
coincident_count = 0;
coincident_count = 0;
}
else if(normal_contact_average)
else if (normal_contact_average)
{
if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
{
if(coincident_count<MAX_COINCIDENT)
{
coincident_normals[coincident_count] = scontact->m_normal;
coincident_count++;
}
}
if (btFabs(pcontact->m_depth - scontact->m_depth) < CONTACT_DIFF_EPSILON)
{
if (coincident_count < MAX_COINCIDENT)
{
coincident_normals[coincident_count] = scontact->m_normal;
coincident_count++;
}
}
}
}
else
{//add new contact
{ //add new contact
if(normal_contact_average && coincident_count>0)
{
pcontact->interpolate_normals(coincident_normals,coincident_count);
coincident_count = 0;
}
if (normal_contact_average && coincident_count > 0)
{
pcontact->interpolate_normals(coincident_normals, coincident_count);
coincident_count = 0;
}
push_back(*scontact);
pcontact = &back();
}
push_back(*scontact);
pcontact = &back();
}
last_key = key;
}
}
void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts)
void gim_contact_array::merge_contacts_unique(const gim_contact_array& contacts)
{
clear();
if(contacts.size()==1)
if (contacts.size() == 1)
{
push_back(contacts.back());
return;
@@ -125,14 +123,14 @@ void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts
GIM_CONTACT average_contact = contacts.back();
for (GUINT i=1;i<contacts.size() ;i++ )
for (GUINT i = 1; i < contacts.size(); i++)
{
average_contact.m_point += contacts[i].m_point;
average_contact.m_normal += contacts[i].m_normal * contacts[i].m_depth;
}
//divide
GREAL divide_average = 1.0f/((GREAL)contacts.size());
GREAL divide_average = 1.0f / ((GREAL)contacts.size());
average_contact.m_point *= divide_average;
@@ -141,6 +139,4 @@ void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts
average_contact.m_depth = average_contact.m_normal.length();
average_contact.m_normal /= average_contact.m_depth;
}

122
src/BulletCollision/Gimpact/gim_contact.h
View File

@@ -36,7 +36,6 @@ email: projectileman@yahoo.com
#include "gim_radixsort.h"
#include "gim_array.h"
/**
Configuration var for applying interpolation of contact normals
*/
@@ -58,90 +57,87 @@ Configuration var for applying interpolation of contact normals
class GIM_CONTACT
{
public:
btVector3 m_point;
btVector3 m_normal;
GREAL m_depth;//Positive value indicates interpenetration
GREAL m_distance;//Padding not for use
GUINT m_feature1;//Face number
GUINT m_feature2;//Face number
btVector3 m_point;
btVector3 m_normal;
GREAL m_depth; //Positive value indicates interpenetration
GREAL m_distance; //Padding not for use
GUINT m_feature1; //Face number
GUINT m_feature2; //Face number
public:
GIM_CONTACT()
{
}
GIM_CONTACT()
{
}
GIM_CONTACT(const GIM_CONTACT & contact):
m_point(contact.m_point),
m_normal(contact.m_normal),
m_depth(contact.m_depth),
m_feature1(contact.m_feature1),
m_feature2(contact.m_feature2)
{
m_point = contact.m_point;
m_normal = contact.m_normal;
m_depth = contact.m_depth;
m_feature1 = contact.m_feature1;
m_feature2 = contact.m_feature2;
}
GIM_CONTACT(const GIM_CONTACT &contact) : m_point(contact.m_point),
m_normal(contact.m_normal),
m_depth(contact.m_depth),
m_feature1(contact.m_feature1),
m_feature2(contact.m_feature2)
{
m_point = contact.m_point;
m_normal = contact.m_normal;
m_depth = contact.m_depth;
m_feature1 = contact.m_feature1;
m_feature2 = contact.m_feature2;
}
GIM_CONTACT(const btVector3 &point,const btVector3 & normal,
GREAL depth, GUINT feature1, GUINT feature2):
m_point(point),
m_normal(normal),
m_depth(depth),
m_feature1(feature1),
m_feature2(feature2)
{
}
GIM_CONTACT(const btVector3 &point, const btVector3 &normal,
GREAL depth, GUINT feature1, GUINT feature2) : m_point(point),
m_normal(normal),
m_depth(depth),
m_feature1(feature1),
m_feature2(feature2)
{
}
//! Calcs key for coord classification
SIMD_FORCE_INLINE GUINT calc_key_contact() const
{
GINT _coords[] = {
(GINT)(m_point[0]*1000.0f+1.0f),
(GINT)(m_point[1]*1333.0f),
(GINT)(m_point[2]*2133.0f+3.0f)};
GUINT _hash=0;
SIMD_FORCE_INLINE GUINT calc_key_contact() const
{
GINT _coords[] = {
(GINT)(m_point[0] * 1000.0f + 1.0f),
(GINT)(m_point[1] * 1333.0f),
(GINT)(m_point[2] * 2133.0f + 3.0f)};
GUINT _hash = 0;
GUINT *_uitmp = (GUINT *)(&_coords[0]);
_hash = *_uitmp;
_uitmp++;
_hash += (*_uitmp)<<4;
_hash += (*_uitmp) << 4;
_uitmp++;
_hash += (*_uitmp)<<8;
_hash += (*_uitmp) << 8;
return _hash;
}
}
SIMD_FORCE_INLINE void interpolate_normals( btVector3 * normals,GUINT normal_count)
{
btVector3 vec_sum(m_normal);
for(GUINT i=0;i<normal_count;i++)
SIMD_FORCE_INLINE void interpolate_normals(btVector3 *normals, GUINT normal_count)
{
btVector3 vec_sum(m_normal);
for (GUINT i = 0; i < normal_count; i++)
{
vec_sum += normals[i];
}
GREAL vec_sum_len = vec_sum.length2();
if(vec_sum_len <CONTACT_DIFF_EPSILON) return;
if (vec_sum_len < CONTACT_DIFF_EPSILON) return;
GIM_INV_SQRT(vec_sum_len,vec_sum_len); // 1/sqrt(vec_sum_len)
m_normal = vec_sum*vec_sum_len;
}
GIM_INV_SQRT(vec_sum_len, vec_sum_len); // 1/sqrt(vec_sum_len)
m_normal = vec_sum * vec_sum_len;
}
};
#endif
class gim_contact_array:public gim_array<GIM_CONTACT>
class gim_contact_array : public gim_array<GIM_CONTACT>
{
public:
gim_contact_array():gim_array<GIM_CONTACT>(64)
gim_contact_array() : gim_array<GIM_CONTACT>(64)
{
}
SIMD_FORCE_INLINE void push_contact(const btVector3 &point,const btVector3 & normal,
GREAL depth, GUINT feature1, GUINT feature2)
SIMD_FORCE_INLINE void push_contact(const btVector3 &point, const btVector3 &normal,
GREAL depth, GUINT feature1, GUINT feature2)
{
push_back_mem();
GIM_CONTACT & newele = back();
GIM_CONTACT &newele = back();
newele.m_point = point;
newele.m_normal = normal;
newele.m_depth = depth;
@@ -150,13 +146,13 @@ public:
}
SIMD_FORCE_INLINE void push_triangle_contacts(
const GIM_TRIANGLE_CONTACT_DATA & tricontact,
GUINT feature1,GUINT feature2)
const GIM_TRIANGLE_CONTACT_DATA &tricontact,
GUINT feature1, GUINT feature2)
{
for(GUINT i = 0;i<tricontact.m_point_count ;i++ )
for (GUINT i = 0; i < tricontact.m_point_count; i++)
{
push_back_mem();
GIM_CONTACT & newele = back();
GIM_CONTACT &newele = back();
newele.m_point = tricontact.m_points[i];
newele.m_normal = tricontact.m_separating_normal;
newele.m_depth = tricontact.m_penetration_depth;
@@ -165,8 +161,8 @@ public:
}
}
void merge_contacts(const gim_contact_array & contacts, bool normal_contact_average = true);
void merge_contacts_unique(const gim_contact_array & contacts);
void merge_contacts(const gim_contact_array &contacts, bool normal_contact_average = true);
void merge_contacts_unique(const gim_contact_array &contacts);
};
#endif // GIM_CONTACT_H_INCLUDED
#endif // GIM_CONTACT_H_INCLUDED

7
src/BulletCollision/Gimpact/gim_geom_types.h
View File

@@ -33,11 +33,8 @@ email: projectileman@yahoo.com
-----------------------------------------------------------------------------
*/
#include "gim_math.h"
//! Short Integer vector 2D
typedef GSHORT vec2s[2];
//! Integer vector 3D
@@ -92,6 +89,4 @@ typedef GREAL quatf[4];
//typedef struct _aabb3f aabb3f;
#endif // GIM_GEOM_TYPES_H_INCLUDED
#endif // GIM_GEOM_TYPES_H_INCLUDED

2
src/BulletCollision/Gimpact/gim_geometry.h
View File

@@ -39,4 +39,4 @@ email: projectileman@yahoo.com
#include "gim_box_collision.h"
#include "gim_tri_collision.h"
#endif // GIM_VECTOR_H_INCLUDED
#endif // GIM_VECTOR_H_INCLUDED

1227
src/BulletCollision/Gimpact/gim_hash_table.h
View File

File diff suppressed because it is too large Load Diff

2263
src/BulletCollision/Gimpact/gim_linear_math.h
View File

File diff suppressed because it is too large Load Diff

107
src/BulletCollision/Gimpact/gim_math.h
View File

@@ -34,8 +34,6 @@ email: projectileman@yahoo.com
#include "LinearMath/btScalar.h"
#define GREAL btScalar
#define GREAL2 double
#define GINT int
@@ -45,8 +43,6 @@ email: projectileman@yahoo.com
#define GINT64 long long
#define GUINT64 unsigned long long
#define G_PI 3.14159265358979f
#define G_HALF_PI 1.5707963f
//267948966
@@ -54,16 +50,14 @@ email: projectileman@yahoo.com
//71795864
#define G_ROOT3 1.73205f
#define G_ROOT2 1.41421f
#define G_UINT_INFINITY 0xffffffff //!< A very very high value
#define G_UINT_INFINITY 0xffffffff //!< A very very high value
#define G_REAL_INFINITY FLT_MAX
#define G_SIGN_BITMASK 0x80000000
#define G_SIGN_BITMASK 0x80000000
#define G_EPSILON SIMD_EPSILON
enum GIM_SCALAR_TYPES
{
G_STYPE_REAL =0,
G_STYPE_REAL = 0,
G_STYPE_REAL2,
G_STYPE_SHORT,
G_STYPE_USHORT,
@@ -73,85 +67,82 @@ enum GIM_SCALAR_TYPES
G_STYPE_UINT64
};
#define G_DEGTORAD(X) ((X)*3.1415926f/180.0f)
#define G_RADTODEG(X) ((X)*180.0f/3.1415926f)
#define G_DEGTORAD(X) ((X)*3.1415926f / 180.0f)
#define G_RADTODEG(X) ((X)*180.0f / 3.1415926f)
//! Integer representation of a floating-point value.
#define GIM_IR(x) ((GUINT&)(x))
#define GIM_IR(x) ((GUINT&)(x))
//! Signed integer representation of a floating-point value.
#define GIM_SIR(x) ((GINT&)(x))
#define GIM_SIR(x) ((GINT&)(x))
//! Absolute integer representation of a floating-point value
#define GIM_AIR(x) (GIM_IR(x)&0x7fffffff)
#define GIM_AIR(x) (GIM_IR(x) & 0x7fffffff)
//! Floating-point representation of an integer value.
#define GIM_FR(x) ((GREAL&)(x))
#define GIM_FR(x) ((GREAL&)(x))
#define GIM_MAX(a,b) (a<b?b:a)
#define GIM_MIN(a,b) (a>b?b:a)
#define GIM_MAX(a, b) (a < b ? b : a)
#define GIM_MIN(a, b) (a > b ? b : a)
#define GIM_MAX3(a,b,c) GIM_MAX(a,GIM_MAX(b,c))
#define GIM_MIN3(a,b,c) GIM_MIN(a,GIM_MIN(b,c))
#define GIM_MAX3(a, b, c) GIM_MAX(a, GIM_MAX(b, c))
#define GIM_MIN3(a, b, c) GIM_MIN(a, GIM_MIN(b, c))
#define GIM_IS_ZERO(value) (value < G_EPSILON && value > -G_EPSILON)
#define GIM_IS_ZERO(value) (value < G_EPSILON && value > -G_EPSILON)
#define GIM_IS_NEGATIVE(value) (value <= -G_EPSILON)
#define GIM_IS_POSISITVE(value) (value >= G_EPSILON)
#define GIM_NEAR_EQUAL(v1,v2) GIM_IS_ZERO((v1-v2))
#define GIM_NEAR_EQUAL(v1, v2) GIM_IS_ZERO((v1 - v2))
///returns a clamped number
#define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
#define GIM_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number))
#define GIM_GREATER(x, y) btFabs(x) > (y)
#define GIM_GREATER(x, y) btFabs(x) > (y)
///Swap numbers
#define GIM_SWAP_NUMBERS(a,b){ \
a = a+b; \
b = a-b; \
a = a-b; \
}\
#define GIM_SWAP_NUMBERS(a, b) \
{ \
a = a + b; \
b = a - b; \
a = a - b; \
}
#define GIM_INV_SQRT(va,isva)\
{\
if(va<=0.0000001f)\
{\
isva = G_REAL_INFINITY;\
}\
else\
{\
GREAL _x = va * 0.5f;\
GUINT _y = 0x5f3759df - ( GIM_IR(va) >> 1);\
isva = GIM_FR(_y);\
isva = isva * ( 1.5f - ( _x * isva * isva ) );\
}\
}\
#define GIM_INV_SQRT(va, isva) \
{ \
if (va <= 0.0000001f) \
{ \
isva = G_REAL_INFINITY; \
} \
else \
{ \
GREAL _x = va * 0.5f; \
GUINT _y = 0x5f3759df - (GIM_IR(va) >> 1); \
isva = GIM_FR(_y); \
isva = isva * (1.5f - (_x * isva * isva)); \
} \
}
#define GIM_SQRT(va,sva)\
{\
GIM_INV_SQRT(va,sva);\
sva = 1.0f/sva;\
}\
#define GIM_SQRT(va, sva) \
{ \
GIM_INV_SQRT(va, sva); \
sva = 1.0f / sva; \
}
//! Computes 1.0f / sqrtf(x). Comes from Quake3. See http://www.magic-software.com/3DGEDInvSqrt.html
inline GREAL gim_inv_sqrt(GREAL f)
{
GREAL r;
GIM_INV_SQRT(f,r);
return r;
GREAL r;
GIM_INV_SQRT(f, r);
return r;
}
inline GREAL gim_sqrt(GREAL f)
{
GREAL r;
GIM_SQRT(f,r);
return r;
GREAL r;
GIM_SQRT(f, r);
return r;
}
#endif // GIM_MATH_H_INCLUDED
#endif // GIM_MATH_H_INCLUDED

69
src/BulletCollision/Gimpact/gim_memory.cpp
View File

@@ -27,7 +27,6 @@ email: projectileman@yahoo.com
-----------------------------------------------------------------------------
*/
#include "gim_memory.h"
#include "stdlib.h"
@@ -40,52 +39,49 @@ static gim_alloca_function *g_allocafn = 0;
static gim_realloc_function *g_reallocfn = 0;
static gim_free_function *g_freefn = 0;
void gim_set_alloc_handler (gim_alloc_function *fn)
void gim_set_alloc_handler(gim_alloc_function *fn)
{
g_allocfn = fn;
g_allocfn = fn;
}
void gim_set_alloca_handler (gim_alloca_function *fn)
void gim_set_alloca_handler(gim_alloca_function *fn)
{
g_allocafn = fn;
g_allocafn = fn;
}
void gim_set_realloc_handler (gim_realloc_function *fn)
void gim_set_realloc_handler(gim_realloc_function *fn)
{
g_reallocfn = fn;
g_reallocfn = fn;
}
void gim_set_free_handler (gim_free_function *fn)
void gim_set_free_handler(gim_free_function *fn)
{
g_freefn = fn;
g_freefn = fn;
}
gim_alloc_function *gim_get_alloc_handler()
{
return g_allocfn;
return g_allocfn;
}
gim_alloca_function *gim_get_alloca_handler()
{
return g_allocafn;
return g_allocafn;
}
gim_realloc_function *gim_get_realloc_handler ()
gim_realloc_function *gim_get_realloc_handler()
{
return g_reallocfn;
return g_reallocfn;
}
gim_free_function *gim_get_free_handler ()
gim_free_function *gim_get_free_handler()
{
return g_freefn;
return g_freefn;
}
void * gim_alloc(size_t size)
void *gim_alloc(size_t size)
{
void * ptr;
void *ptr;
if (g_allocfn)
{
ptr = g_allocfn(size);
@@ -93,27 +89,29 @@ void * gim_alloc(size_t size)
else
{
#ifdef GIM_SIMD_MEMORY
ptr = btAlignedAlloc(size,16);
ptr = btAlignedAlloc(size, 16);
#else
ptr = malloc(size);
#endif
}
return ptr;
return ptr;
}
void * gim_alloca(size_t size)
void *gim_alloca(size_t size)
{
if (g_allocafn) return g_allocafn(size); else return gim_alloc(size);
if (g_allocafn)
return g_allocafn(size);
else
return gim_alloc(size);
}
void * gim_realloc(void *ptr, size_t oldsize, size_t newsize)
void *gim_realloc(void *ptr, size_t oldsize, size_t newsize)
{
void * newptr = gim_alloc(newsize);
size_t copysize = oldsize<newsize?oldsize:newsize;
gim_simd_memcpy(newptr,ptr,copysize);
gim_free(ptr);
return newptr;
void *newptr = gim_alloc(newsize);
size_t copysize = oldsize < newsize ? oldsize : newsize;
gim_simd_memcpy(newptr, ptr, copysize);
gim_free(ptr);
return newptr;
}
void gim_free(void *ptr)
@@ -121,15 +119,14 @@ void gim_free(void *ptr)
if (!ptr) return;
if (g_freefn)
{
g_freefn(ptr);
g_freefn(ptr);
}
else
{
#ifdef GIM_SIMD_MEMORY
#ifdef GIM_SIMD_MEMORY
btAlignedFree(ptr);
#else
#else
free(ptr);
#endif
#endif
}
}

165
src/BulletCollision/Gimpact/gim_memory.h
View File

@@ -32,93 +32,84 @@ email: projectileman@yahoo.com
-----------------------------------------------------------------------------
*/
#include "gim_math.h"
#include <string.h>
#ifdef PREFETCH
#include <xmmintrin.h> // for prefetch
#define pfval 64
#define pfval2 128
#include <xmmintrin.h> // for prefetch
#define pfval 64
#define pfval2 128
//! Prefetch 64
#define pf(_x,_i) _mm_prefetch((void *)(_x + _i + pfval), 0)
#define pf(_x, _i) _mm_prefetch((void *)(_x + _i + pfval), 0)
//! Prefetch 128
#define pf2(_x,_i) _mm_prefetch((void *)(_x + _i + pfval2), 0)
#define pf2(_x, _i) _mm_prefetch((void *)(_x + _i + pfval2), 0)
#else
//! Prefetch 64
#define pf(_x,_i)
#define pf(_x, _i)
//! Prefetch 128
#define pf2(_x,_i)
#define pf2(_x, _i)
#endif
///Functions for manip packed arrays of numbers
#define GIM_COPY_ARRAYS(dest_array,source_array,element_count)\
{\
for (GUINT _i_=0;_i_<element_count ;++_i_)\
{\
dest_array[_i_] = source_array[_i_];\
}\
}\
#define GIM_COPY_ARRAYS(dest_array, source_array, element_count) \
{ \
for (GUINT _i_ = 0; _i_ < element_count; ++_i_) \
{ \
dest_array[_i_] = source_array[_i_]; \
} \
}
#define GIM_COPY_ARRAYS_1(dest_array,source_array,element_count,copy_macro)\
{\
for (GUINT _i_=0;_i_<element_count ;++_i_)\
{\
copy_macro(dest_array[_i_],source_array[_i_]);\
}\
}\
#define GIM_COPY_ARRAYS_1(dest_array, source_array, element_count, copy_macro) \
{ \
for (GUINT _i_ = 0; _i_ < element_count; ++_i_) \
{ \
copy_macro(dest_array[_i_], source_array[_i_]); \
} \
}
#define GIM_ZERO_ARRAY(array, element_count) \
{ \
for (GUINT _i_ = 0; _i_ < element_count; ++_i_) \
{ \
array[_i_] = 0; \
} \
}
#define GIM_ZERO_ARRAY(array,element_count)\
{\
for (GUINT _i_=0;_i_<element_count ;++_i_)\
{\
array[_i_] = 0;\
}\
}\
#define GIM_CONSTANT_ARRAY(array,element_count,constant)\
{\
for (GUINT _i_=0;_i_<element_count ;++_i_)\
{\
array[_i_] = constant;\
}\
}\
#define GIM_CONSTANT_ARRAY(array, element_count, constant) \
{ \
for (GUINT _i_ = 0; _i_ < element_count; ++_i_) \
{ \
array[_i_] = constant; \
} \
}
///Function prototypes to allocate and free memory.
typedef void * gim_alloc_function (size_t size);
typedef void * gim_alloca_function (size_t size);//Allocs on the heap
typedef void * gim_realloc_function (void *ptr, size_t oldsize, size_t newsize);
typedef void gim_free_function (void *ptr);
typedef void *gim_alloc_function(size_t size);
typedef void *gim_alloca_function(size_t size); //Allocs on the heap
typedef void *gim_realloc_function(void *ptr, size_t oldsize, size_t newsize);
typedef void gim_free_function(void *ptr);
///Memory Function Handlers
///set new memory management functions. if fn is 0, the default handlers are used.
void gim_set_alloc_handler (gim_alloc_function *fn);
void gim_set_alloca_handler (gim_alloca_function *fn);
void gim_set_realloc_handler (gim_realloc_function *fn);
void gim_set_free_handler (gim_free_function *fn);
void gim_set_alloc_handler(gim_alloc_function *fn);
void gim_set_alloca_handler(gim_alloca_function *fn);
void gim_set_realloc_handler(gim_realloc_function *fn);
void gim_set_free_handler(gim_free_function *fn);
///get current memory management functions.
gim_alloc_function *gim_get_alloc_handler (void);
gim_alloc_function *gim_get_alloc_handler(void);
gim_alloca_function *gim_get_alloca_handler(void);
gim_realloc_function *gim_get_realloc_handler (void);
gim_free_function *gim_get_free_handler (void);
gim_realloc_function *gim_get_realloc_handler(void);
gim_free_function *gim_get_free_handler(void);
///Standar Memory functions
void * gim_alloc(size_t size);
void * gim_alloca(size_t size);
void * gim_realloc(void *ptr, size_t oldsize, size_t newsize);
void *gim_alloc(size_t size);
void *gim_alloca(size_t size);
void *gim_realloc(void *ptr, size_t oldsize, size_t newsize);
void gim_free(void *ptr);
#if defined (_WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__)
#define GIM_SIMD_MEMORY 1
#if defined(_WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__)
#define GIM_SIMD_MEMORY 1
#endif
//! SIMD POINTER INTEGER
@@ -126,11 +117,10 @@ void gim_free(void *ptr);
//! SIMD INTEGER SIZE
#define SIMD_T_SIZE sizeof(SIMD_T)
inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize)
inline void gim_simd_memcpy(void *dst, const void *src, size_t copysize)
{
#ifdef GIM_SIMD_MEMORY
/*
/*
//'long long int' is incompatible with visual studio 6...
//copy words
SIMD_T * ui_src_ptr = (SIMD_T *)src;
@@ -143,48 +133,45 @@ inline void gim_simd_memcpy(void * dst, const void * src, size_t copysize)
if(copysize==0) return;
*/
char * c_src_ptr = (char *)src;
char * c_dst_ptr = (char *)dst;
while(copysize>0)
{
*(c_dst_ptr++) = *(c_src_ptr++);
copysize--;
}
return;
char *c_src_ptr = (char *)src;
char *c_dst_ptr = (char *)dst;
while (copysize > 0)
{
*(c_dst_ptr++) = *(c_src_ptr++);
copysize--;
}
return;
#else
memcpy(dst,src,copysize);
memcpy(dst, src, copysize);
#endif
}
template<class T>
inline void gim_swap_elements(T* _array,size_t _i,size_t _j)
template <class T>
inline void gim_swap_elements(T *_array, size_t _i, size_t _j)
{
T _e_tmp_ = _array[_i];
_array[_i] = _array[_j];
_array[_j] = _e_tmp_;
}
template<class T>
inline void gim_swap_elements_memcpy(T* _array,size_t _i,size_t _j)
template <class T>
inline void gim_swap_elements_memcpy(T *_array, size_t _i, size_t _j)
{
char _e_tmp_[sizeof(T)];
gim_simd_memcpy(_e_tmp_,&_array[_i],sizeof(T));
gim_simd_memcpy(&_array[_i],&_array[_j],sizeof(T));
gim_simd_memcpy(&_array[_j],_e_tmp_,sizeof(T));
gim_simd_memcpy(_e_tmp_, &_array[_i], sizeof(T));
gim_simd_memcpy(&_array[_i], &_array[_j], sizeof(T));
gim_simd_memcpy(&_array[_j], _e_tmp_, sizeof(T));
}
template <int SIZE>
inline void gim_swap_elements_ptr(char * _array,size_t _i,size_t _j)
inline void gim_swap_elements_ptr(char *_array, size_t _i, size_t _j)
{
char _e_tmp_[SIZE];
_i*=SIZE;
_j*=SIZE;
gim_simd_memcpy(_e_tmp_,_array+_i,SIZE);
gim_simd_memcpy(_array+_i,_array+_j,SIZE);
gim_simd_memcpy(_array+_j,_e_tmp_,SIZE);
_i *= SIZE;
_j *= SIZE;
gim_simd_memcpy(_e_tmp_, _array + _i, SIZE);
gim_simd_memcpy(_array + _i, _array + _j, SIZE);
gim_simd_memcpy(_array + _j, _e_tmp_, SIZE);
}
#endif // GIM_MEMORY_H_INCLUDED
#endif // GIM_MEMORY_H_INCLUDED

214
src/BulletCollision/Gimpact/gim_radixsort.h
View File

@@ -40,24 +40,22 @@ email: projectileman@yahoo.com
//! Prototype for comparators
class less_comparator
{
public:
template<class T,class Z>
inline int operator() ( const T& a, const Z& b )
public:
template <class T, class Z>
inline int operator()(const T& a, const Z& b)
{
return ( a<b?-1:(a>b?1:0));
return (a < b ? -1 : (a > b ? 1 : 0));
}
};
//! Prototype for comparators
class integer_comparator
{
public:
template<class T>
inline int operator() ( const T& a, const T& b )
public:
template <class T>
inline int operator()(const T& a, const T& b)
{
return (int)(a-b);
return (int)(a - b);
}
};
@@ -65,20 +63,19 @@ class integer_comparator
class uint_key_func
{
public:
template<class T>
inline GUINT operator()( const T& a)
template <class T>
inline GUINT operator()(const T& a)
{
return (GUINT)a;
}
};
//!Prototype for copying elements
class copy_elements_func
{
public:
template<class T>
inline void operator()(T& a,T& b)
template <class T>
inline void operator()(T& a, T& b)
{
a = b;
}
@@ -88,34 +85,33 @@ public:
class memcopy_elements_func
{
public:
template<class T>
inline void operator()(T& a,T& b)
template <class T>
inline void operator()(T& a, T& b)
{
gim_simd_memcpy(&a,&b,sizeof(T));
gim_simd_memcpy(&a, &b, sizeof(T));
}
};
//! @{
struct GIM_RSORT_TOKEN
{
GUINT m_key;
GUINT m_value;
GIM_RSORT_TOKEN()
{
}
GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken)
{
m_key = rtoken.m_key;
m_value = rtoken.m_value;
}
GUINT m_key;
GUINT m_value;
GIM_RSORT_TOKEN()
{
}
GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken)
{
m_key = rtoken.m_key;
m_value = rtoken.m_value;
}
inline bool operator <(const GIM_RSORT_TOKEN& other) const
inline bool operator<(const GIM_RSORT_TOKEN& other) const
{
return (m_key < other.m_key);
}
inline bool operator >(const GIM_RSORT_TOKEN& other) const
inline bool operator>(const GIM_RSORT_TOKEN& other) const
{
return (m_key > other.m_key);
}
@@ -124,33 +120,28 @@ struct GIM_RSORT_TOKEN
//! Prototype for comparators
class GIM_RSORT_TOKEN_COMPARATOR
{
public:
inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b )
public:
inline int operator()(const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b)
{
return (int)((a.m_key) - (b.m_key));
}
};
#define kHist 2048
// ---- utils for accessing 11-bit quantities
#define D11_0(x) (x & 0x7FF)
#define D11_1(x) (x >> 11 & 0x7FF)
#define D11_2(x) (x >> 22 )
#define D11_0(x) (x & 0x7FF)
#define D11_1(x) (x >> 11 & 0x7FF)
#define D11_2(x) (x >> 22)
///Radix sort for unsigned integer keys
inline void gim_radix_sort_rtokens(
GIM_RSORT_TOKEN * array,
GIM_RSORT_TOKEN * sorted, GUINT element_count)
GIM_RSORT_TOKEN* array,
GIM_RSORT_TOKEN* sorted, GUINT element_count)
{
GUINT i;
GUINT b0[kHist * 3];
GUINT *b1 = b0 + kHist;
GUINT *b2 = b1 + kHist;
GUINT* b1 = b0 + kHist;
GUINT* b2 = b1 + kHist;
for (i = 0; i < kHist * 3; ++i)
{
b0[i] = 0;
@@ -159,10 +150,10 @@ inline void gim_radix_sort_rtokens(
GUINT pos;
for (i = 0; i < element_count; ++i)
{
fi = array[i].m_key;
b0[D11_0(fi)] ++;
b1[D11_1(fi)] ++;
b2[D11_2(fi)] ++;
fi = array[i].m_key;
b0[D11_0(fi)]++;
b1[D11_1(fi)]++;
b2[D11_2(fi)]++;
}
{
GUINT sum0 = 0, sum1 = 0, sum2 = 0;
@@ -182,7 +173,7 @@ inline void gim_radix_sort_rtokens(
}
for (i = 0; i < element_count; ++i)
{
fi = array[i].m_key;
fi = array[i].m_key;
pos = D11_0(fi);
pos = ++b0[pos];
sorted[pos].m_key = array[i].m_key;
@@ -190,7 +181,7 @@ inline void gim_radix_sort_rtokens(
}
for (i = 0; i < element_count; ++i)
{
fi = sorted[i].m_key;
fi = sorted[i].m_key;
pos = D11_1(fi);
pos = ++b1[pos];
array[pos].m_key = sorted[i].m_key;
@@ -198,7 +189,7 @@ inline void gim_radix_sort_rtokens(
}
for (i = 0; i < element_count; ++i)
{
fi = array[i].m_key;
fi = array[i].m_key;
pos = D11_2(fi);
pos = ++b2[pos];
sorted[pos].m_key = array[i].m_key;
@@ -206,9 +197,6 @@ inline void gim_radix_sort_rtokens(
}
}
/// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN
/*!
*\param array Array of elements to sort
@@ -216,21 +204,21 @@ inline void gim_radix_sort_rtokens(
*\param element_count element count
*\param uintkey_macro Functor which retrieves the integer representation of an array element
*/
template<typename T, class GETKEY_CLASS>
template <typename T, class GETKEY_CLASS>
void gim_radix_sort_array_tokens(
T* array ,
GIM_RSORT_TOKEN * sorted_tokens,
GUINT element_count,GETKEY_CLASS uintkey_macro)
T* array,
GIM_RSORT_TOKEN* sorted_tokens,
GUINT element_count, GETKEY_CLASS uintkey_macro)
{
GIM_RSORT_TOKEN * _unsorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
for (GUINT _i=0;_i<element_count;++_i)
{
_unsorted[_i].m_key = uintkey_macro(array[_i]);
_unsorted[_i].m_value = _i;
}
gim_radix_sort_rtokens(_unsorted,sorted_tokens,element_count);
gim_free(_unsorted);
gim_free(_unsorted);
GIM_RSORT_TOKEN* _unsorted = (GIM_RSORT_TOKEN*)gim_alloc(sizeof(GIM_RSORT_TOKEN) * element_count);
for (GUINT _i = 0; _i < element_count; ++_i)
{
_unsorted[_i].m_key = uintkey_macro(array[_i]);
_unsorted[_i].m_value = _i;
}
gim_radix_sort_rtokens(_unsorted, sorted_tokens, element_count);
gim_free(_unsorted);
gim_free(_unsorted);
}
/// Sorts array in place. For generic use
@@ -241,21 +229,21 @@ void gim_radix_sort_array_tokens(
\param get_uintkey_macro Macro for extract the Integer value of the element. Similar to SIMPLE_GET_UINTKEY
\param copy_elements_macro Macro for copy elements, similar to SIMPLE_COPY_ELEMENTS
*/
template<typename T, class GETKEY_CLASS, class COPY_CLASS>
template <typename T, class GETKEY_CLASS, class COPY_CLASS>
void gim_radix_sort(
T * array, GUINT element_count,
T* array, GUINT element_count,
GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro)
{
GIM_RSORT_TOKEN * _sorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
gim_radix_sort_array_tokens(array,_sorted,element_count,get_uintkey_macro);
T * _original_array = (T *) gim_alloc(sizeof(T)*element_count);
gim_simd_memcpy(_original_array,array,sizeof(T)*element_count);
for (GUINT _i=0;_i<element_count;++_i)
{
copy_elements_macro(array[_i],_original_array[_sorted[_i].m_value]);
}
gim_free(_original_array);
gim_free(_sorted);
GIM_RSORT_TOKEN* _sorted = (GIM_RSORT_TOKEN*)gim_alloc(sizeof(GIM_RSORT_TOKEN) * element_count);
gim_radix_sort_array_tokens(array, _sorted, element_count, get_uintkey_macro);
T* _original_array = (T*)gim_alloc(sizeof(T) * element_count);
gim_simd_memcpy(_original_array, array, sizeof(T) * element_count);
for (GUINT _i = 0; _i < element_count; ++_i)
{
copy_elements_macro(array[_i], _original_array[_sorted[_i].m_value]);
}
gim_free(_original_array);
gim_free(_sorted);
}
//! Failsafe Iterative binary search,
@@ -269,20 +257,20 @@ If the element is not found, it returns the nearest upper element position, may
\param _found If true the value has found. Boolean
\param _result_index the index of the found element, or if not found then it will get the index of the closest bigger value
*/
template<class T, typename KEYCLASS, typename COMP_CLASS>
bool gim_binary_search_ex(
const T* _array, GUINT _start_i,
GUINT _end_i,GUINT & _result_index,
const KEYCLASS & _search_key,
COMP_CLASS _comp_macro)
template <class T, typename KEYCLASS, typename COMP_CLASS>
bool gim_binary_search_ex(
const T* _array, GUINT _start_i,
GUINT _end_i, GUINT& _result_index,
const KEYCLASS& _search_key,
COMP_CLASS _comp_macro)
{
GUINT _k;
int _comp_result;
GUINT _i = _start_i;
GUINT _j = _end_i+1;
GUINT _j = _end_i + 1;
while (_i < _j)
{
_k = (_j+_i-1)/2;
_k = (_j + _i - 1) / 2;
_comp_result = _comp_macro(_array[_k], _search_key);
if (_comp_result == 0)
{
@@ -291,7 +279,7 @@ bool gim_binary_search_ex(
}
else if (_comp_result < 0)
{
_i = _k+1;
_i = _k + 1;
}
else
{
@@ -302,8 +290,6 @@ bool gim_binary_search_ex(
return false;
}
//! Failsafe Iterative binary search,Template version
/*!
If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
@@ -314,26 +300,26 @@ If the element is not found, it returns the nearest upper element position, may
\param _result_index the index of the found element, or if not found then it will get the index of the closest bigger value
\return true if found, else false
*/
template<class T>
template <class T>
bool gim_binary_search(
const T*_array,GUINT _start_i,
GUINT _end_i,const T & _search_key,
GUINT & _result_index)
const T* _array, GUINT _start_i,
GUINT _end_i, const T& _search_key,
GUINT& _result_index)
{
GUINT _i = _start_i;
GUINT _j = _end_i+1;
GUINT _j = _end_i + 1;
GUINT _k;
while(_i < _j)
while (_i < _j)
{
_k = (_j+_i-1)/2;
if(_array[_k]==_search_key)
_k = (_j + _i - 1) / 2;
if (_array[_k] == _search_key)
{
_result_index = _k;
return true;
}
else if (_array[_k]<_search_key)
else if (_array[_k] < _search_key)
{
_i = _k+1;
_i = _k + 1;
}
else
{
@@ -344,27 +330,25 @@ bool gim_binary_search(
return false;
}
///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
template <typename T, typename COMP_CLASS>
void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc)
void gim_down_heap(T* pArr, GUINT k, GUINT n, COMP_CLASS CompareFunc)
{
/* PRE: a[k+1..N] is a heap */
/* POST: a[k..N] is a heap */
T temp = pArr[k - 1];
/* k has child(s) */
while (k <= n/2)
while (k <= n / 2)
{
int child = 2*k;
int child = 2 * k;
if ((child < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0)
if ((child < (int)n) && CompareFunc(pArr[child - 1], pArr[child]) < 0)
{
child++;
}
/* pick larger child */
if (CompareFunc(temp , pArr[child - 1])<0)
if (CompareFunc(temp, pArr[child - 1]) < 0)
{
/* move child up */
pArr[k - 1] = pArr[child - 1];
@@ -378,29 +362,25 @@ void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc)
pArr[k - 1] = temp;
} /*downHeap*/
template <typename T, typename COMP_CLASS>
void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc)
void gim_heap_sort(T* pArr, GUINT element_count, COMP_CLASS CompareFunc)
{
/* sort a[0..N-1], N.B. 0 to N-1 */
GUINT k;
GUINT n = element_count;
for (k = n/2; k > 0; k--)
for (k = n / 2; k > 0; k--)
{
gim_down_heap(pArr, k, n, CompareFunc);
}
/* a[1..N] is now a heap */
while ( n>=2 )
while (n >= 2)
{
gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */
gim_swap_elements(pArr, 0, n - 1); /* largest of a[0..n-1] */
--n;
/* restore a[1..i-1] heap */
gim_down_heap(pArr, 1, n, CompareFunc);
}
}
#endif // GIM_RADIXSORT_H_INCLUDED
#endif // GIM_RADIXSORT_H_INCLUDED

321
src/BulletCollision/Gimpact/gim_tri_collision.cpp
View File

@@ -33,15 +33,13 @@ email: projectileman@yahoo.com
#include "gim_tri_collision.h"
#define TRI_LOCAL_EPSILON 0.000001f
#define MIN_EDGE_EDGE_DIS 0.00001f
class GIM_TRIANGLE_CALCULATION_CACHE
{
public:
GREAL margin;
GREAL margin;
btVector3 tu_vertices[3];
btVector3 tv_vertices[3];
btVector4 tu_plane;
@@ -55,46 +53,47 @@ public:
GREAL du0du2;
GREAL dv[4];
GREAL dv0dv1;
GREAL dv0dv2;
GREAL dv0dv2;
btVector3 temp_points[MAX_TRI_CLIPPING];
btVector3 temp_points1[MAX_TRI_CLIPPING];
btVector3 contact_points[MAX_TRI_CLIPPING];
//! if returns false, the faces are paralele
SIMD_FORCE_INLINE bool compute_intervals(
const GREAL &D0,
const GREAL &D1,
const GREAL &D2,
const GREAL &D0D1,
const GREAL &D0D2,
GREAL & scale_edge0,
GREAL & scale_edge1,
GUINT &edge_index0,
GUINT &edge_index1)
const GREAL &D0,
const GREAL &D1,
const GREAL &D2,
const GREAL &D0D1,
const GREAL &D0D2,
GREAL &scale_edge0,
GREAL &scale_edge1,
GUINT &edge_index0,
GUINT &edge_index1)
{
if(D0D1>0.0f)
if (D0D1 > 0.0f)
{
/* here we know that D0D2<=0.0 */
/* that is D0, D1 are on the same side, D2 on the other or on the plane */
scale_edge0 = -D2/(D0-D2);
scale_edge1 = -D1/(D2-D1);
edge_index0 = 2;edge_index1 = 1;
scale_edge0 = -D2 / (D0 - D2);
scale_edge1 = -D1 / (D2 - D1);
edge_index0 = 2;
edge_index1 = 1;
}
else if(D0D2>0.0f)
else if (D0D2 > 0.0f)
{
/* here we know that d0d1<=0.0 */
scale_edge0 = -D0/(D1-D0);
scale_edge1 = -D1/(D2-D1);
edge_index0 = 0;edge_index1 = 1;
scale_edge0 = -D0 / (D1 - D0);
scale_edge1 = -D1 / (D2 - D1);
edge_index0 = 0;
edge_index1 = 1;
}
else if(D1*D2>0.0f || D0!=0.0f)
else if (D1 * D2 > 0.0f || D0 != 0.0f)
{
/* here we know that d0d1<=0.0 or that D0!=0.0 */
scale_edge0 = -D0/(D1-D0);
scale_edge1 = -D2/(D0-D2);
edge_index0 = 0 ;edge_index1 = 2;
scale_edge0 = -D0 / (D1 - D0);
scale_edge1 = -D2 / (D0 - D2);
edge_index0 = 0;
edge_index1 = 2;
}
else
{
@@ -103,46 +102,44 @@ public:
return true;
}
//! clip triangle
/*!
*/
SIMD_FORCE_INLINE GUINT clip_triangle(
const btVector4 & tri_plane,
const btVector3 * tripoints,
const btVector3 * srcpoints,
btVector3 * clip_points)
const btVector4 &tri_plane,
const btVector3 *tripoints,
const btVector3 *srcpoints,
btVector3 *clip_points)
{
// edge 0
btVector4 edgeplane;
EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
EDGE_PLANE(tripoints[0], tripoints[1], tri_plane, edgeplane);
GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
edgeplane, srcpoints[0], srcpoints[1], srcpoints[2], temp_points);
if(clipped_count == 0) return 0;
if (clipped_count == 0) return 0;
// edge 1
EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
EDGE_PLANE(tripoints[1], tripoints[2], tri_plane, edgeplane);
clipped_count = PLANE_CLIP_POLYGON3D(
edgeplane,temp_points,clipped_count,temp_points1);
edgeplane, temp_points, clipped_count, temp_points1);
if(clipped_count == 0) return 0;
if (clipped_count == 0) return 0;
// edge 2
EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
EDGE_PLANE(tripoints[2], tripoints[0], tri_plane, edgeplane);
clipped_count = PLANE_CLIP_POLYGON3D(
edgeplane,temp_points1,clipped_count,clip_points);
edgeplane, temp_points1, clipped_count, clip_points);
return clipped_count;
/*GUINT i0 = (tri_plane.closestAxis()+1)%3;
GUINT i1 = (i0+1)%3;
// edge 0
@@ -172,13 +169,13 @@ public:
}
SIMD_FORCE_INLINE void sort_isect(
GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
GREAL &isect0, GREAL &isect1, GUINT &e0, GUINT &e1, btVector3 &vec0, btVector3 &vec1)
{
if(isect1<isect0)
if (isect1 < isect0)
{
//swap
GIM_SWAP_NUMBERS(isect0,isect1);
GIM_SWAP_NUMBERS(e0,e1);
GIM_SWAP_NUMBERS(isect0, isect1);
GIM_SWAP_NUMBERS(e0, e1);
btVector3 tmp = vec0;
vec0 = vec1;
vec1 = tmp;
@@ -202,53 +199,52 @@ public:
// Compute direction of intersection line
edge_edge_dir = tu_plane.cross(tv_plane);
GREAL Dlen;
VEC_LENGTH(edge_edge_dir,Dlen);
VEC_LENGTH(edge_edge_dir, Dlen);
if(Dlen<0.0001)
if (Dlen < 0.0001)
{
return 0; //faces near paralele
return 0; //faces near paralele
}
edge_edge_dir*= 1/Dlen;//normalize
edge_edge_dir *= 1 / Dlen; //normalize
// Compute interval for triangle 1
GUINT tu_e0,tu_e1;//edge indices
GREAL tu_scale_e0,tu_scale_e1;//edge scale
if(!compute_intervals(du[0],du[1],du[2],
du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
GUINT tu_e0, tu_e1; //edge indices
GREAL tu_scale_e0, tu_scale_e1; //edge scale
if (!compute_intervals(du[0], du[1], du[2],
du0du1, du0du2, tu_scale_e0, tu_scale_e1, tu_e0, tu_e1)) return 0;
// Compute interval for triangle 2
GUINT tv_e0,tv_e1;//edge indices
GREAL tv_scale_e0,tv_scale_e1;//edge scale
GUINT tv_e0, tv_e1; //edge indices
GREAL tv_scale_e0, tv_scale_e1; //edge scale
if(!compute_intervals(dv[0],dv[1],dv[2],
dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
if (!compute_intervals(dv[0], dv[1], dv[2],
dv0dv1, dv0dv2, tv_scale_e0, tv_scale_e1, tv_e0, tv_e1)) return 0;
//proyected vertices
btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0 + 1) % 3], tu_scale_e0);
btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1 + 1) % 3], tu_scale_e1);
btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0 + 1) % 3], tv_scale_e0);
btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1 + 1) % 3], tv_scale_e1);
//proyected intervals
GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
GREAL isect_u[] = {up_e0.dot(edge_edge_dir), up_e1.dot(edge_edge_dir)};
GREAL isect_v[] = {vp_e0.dot(edge_edge_dir), vp_e1.dot(edge_edge_dir)};
sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
sort_isect(isect_u[0], isect_u[1], tu_e0, tu_e1, up_e0, up_e1);
sort_isect(isect_v[0], isect_v[1], tv_e0, tv_e1, vp_e0, vp_e1);
const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
const GREAL midpoint_u = 0.5f * (isect_u[0] + isect_u[1]); // midpoint
const GREAL midpoint_v = 0.5f * (isect_v[0] + isect_v[1]); // midpoint
if(midpoint_u<midpoint_v)
if (midpoint_u < midpoint_v)
{
if(isect_u[1]>=isect_v[1]) // face U casts face V
if (isect_u[1] >= isect_v[1]) // face U casts face V
{
return 1;
}
else if(isect_v[0]<=isect_u[0]) // face V casts face U
else if (isect_v[0] <= isect_u[0]) // face V casts face U
{
return 2;
}
@@ -257,32 +253,31 @@ public:
closest_point_v = vp_e0;
// calc edges and separation
if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
if (isect_u[1] + MIN_EDGE_EDGE_DIS < isect_v[0]) //calc distance between two lines instead
{
SEGMENT_COLLISION(
tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
tu_vertices[tu_e1], tu_vertices[(tu_e1 + 1) % 3],
tv_vertices[tv_e0], tv_vertices[(tv_e0 + 1) % 3],
closest_point_u,
closest_point_v);
edge_edge_dir = closest_point_u-closest_point_v;
VEC_LENGTH(edge_edge_dir,distances[2]);
edge_edge_dir *= 1.0f/distances[2];// normalize
edge_edge_dir = closest_point_u - closest_point_v;
VEC_LENGTH(edge_edge_dir, distances[2]);
edge_edge_dir *= 1.0f / distances[2]; // normalize
}
else
{
distances[2] = isect_v[0]-isect_u[1];//distance negative
//edge_edge_dir *= -1.0f; //normal pointing from V to U
distances[2] = isect_v[0] - isect_u[1]; //distance negative
//edge_edge_dir *= -1.0f; //normal pointing from V to U
}
}
else
{
if(isect_v[1]>=isect_u[1]) // face V casts face U
if (isect_v[1] >= isect_u[1]) // face V casts face U
{
return 2;
}
else if(isect_u[0]<=isect_v[0]) // face U casts face V
else if (isect_u[0] <= isect_v[0]) // face U casts face V
{
return 1;
}
@@ -291,41 +286,39 @@ public:
closest_point_v = vp_e1;
// calc edges and separation
if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
if (isect_v[1] + MIN_EDGE_EDGE_DIS < isect_u[0]) //calc distance between two lines instead
{
SEGMENT_COLLISION(
tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
tu_vertices[tu_e0], tu_vertices[(tu_e0 + 1) % 3],
tv_vertices[tv_e1], tv_vertices[(tv_e1 + 1) % 3],
closest_point_u,
closest_point_v);
edge_edge_dir = closest_point_u-closest_point_v;
VEC_LENGTH(edge_edge_dir,distances[2]);
edge_edge_dir *= 1.0f/distances[2];// normalize
edge_edge_dir = closest_point_u - closest_point_v;
VEC_LENGTH(edge_edge_dir, distances[2]);
edge_edge_dir *= 1.0f / distances[2]; // normalize
}
else
{
distances[2] = isect_u[0]-isect_v[1];//distance negative
//edge_edge_dir *= -1.0f; //normal pointing from V to U
distances[2] = isect_u[0] - isect_v[1]; //distance negative
//edge_edge_dir *= -1.0f; //normal pointing from V to U
}
}
return 3;
}
//! collides by two sides
SIMD_FORCE_INLINE bool triangle_collision(
const btVector3 & u0,
const btVector3 & u1,
const btVector3 & u2,
GREAL margin_u,
const btVector3 & v0,
const btVector3 & v1,
const btVector3 & v2,
GREAL margin_v,
GIM_TRIANGLE_CONTACT_DATA & contacts)
const btVector3 &u0,
const btVector3 &u1,
const btVector3 &u2,
GREAL margin_u,
const btVector3 &v0,
const btVector3 &v1,
const btVector3 &v2,
GREAL margin_v,
GIM_TRIANGLE_CONTACT_DATA &contacts)
{
margin = margin_u + margin_v;
tu_vertices[0] = u0;
@@ -339,103 +332,99 @@ public:
//create planes
// plane v vs U points
TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);
TRIANGLE_PLANE(tv_vertices[0], tv_vertices[1], tv_vertices[2], tv_plane);
du[0] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[0]);
du[1] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[1]);
du[2] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[2]);
du0du1 = du[0] * du[1];
du0du2 = du[0] * du[2];
if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
if (du0du1 > 0.0f && du0du2 > 0.0f) // same sign on all of them + not equal 0 ?
{
if(du[0]<0) //we need test behind the triangle plane
if (du[0] < 0) //we need test behind the triangle plane
{
distances[0] = GIM_MAX3(du[0],du[1],du[2]);
distances[0] = GIM_MAX3(du[0], du[1], du[2]);
distances[0] = -distances[0];
if(distances[0]>margin) return false; //never intersect
if (distances[0] > margin) return false; //never intersect
//reorder triangle v
VEC_SWAP(tv_vertices[0],tv_vertices[1]);
VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
VEC_SWAP(tv_vertices[0], tv_vertices[1]);
VEC_SCALE_4(tv_plane, -1.0f, tv_plane);
}
else
{
distances[0] = GIM_MIN3(du[0],du[1],du[2]);
if(distances[0]>margin) return false; //never intersect
distances[0] = GIM_MIN3(du[0], du[1], du[2]);
if (distances[0] > margin) return false; //never intersect
}
}
else
{
//Look if we need to invert the triangle
distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
distances[0] = (du[0] + du[1] + du[2]) / 3.0f; //centroid
if(distances[0]<0.0f)
if (distances[0] < 0.0f)
{
//reorder triangle v
VEC_SWAP(tv_vertices[0],tv_vertices[1]);
VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
VEC_SWAP(tv_vertices[0], tv_vertices[1]);
VEC_SCALE_4(tv_plane, -1.0f, tv_plane);
distances[0] = GIM_MAX3(du[0],du[1],du[2]);
distances[0] = GIM_MAX3(du[0], du[1], du[2]);
distances[0] = -distances[0];
}
else
{
distances[0] = GIM_MIN3(du[0],du[1],du[2]);
distances[0] = GIM_MIN3(du[0], du[1], du[2]);
}
}
// plane U vs V points
TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
TRIANGLE_PLANE(tu_vertices[0], tu_vertices[1], tu_vertices[2], tu_plane);
dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);
dv[0] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[0]);
dv[1] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[1]);
dv[2] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[2]);
dv0dv1 = dv[0] * dv[1];
dv0dv2 = dv[0] * dv[2];
if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
if (dv0dv1 > 0.0f && dv0dv2 > 0.0f) // same sign on all of them + not equal 0 ?
{
if(dv[0]<0) //we need test behind the triangle plane
if (dv[0] < 0) //we need test behind the triangle plane
{
distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]);
distances[1] = -distances[1];
if(distances[1]>margin) return false; //never intersect
if (distances[1] > margin) return false; //never intersect
//reorder triangle u
VEC_SWAP(tu_vertices[0],tu_vertices[1]);
VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
VEC_SWAP(tu_vertices[0], tu_vertices[1]);
VEC_SCALE_4(tu_plane, -1.0f, tu_plane);
}
else
{
distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
if(distances[1]>margin) return false; //never intersect
distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]);
if (distances[1] > margin) return false; //never intersect
}
}
else
{
//Look if we need to invert the triangle
distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
distances[1] = (dv[0] + dv[1] + dv[2]) / 3.0f; //centroid
if(distances[1]<0.0f)
if (distances[1] < 0.0f)
{
//reorder triangle v
VEC_SWAP(tu_vertices[0],tu_vertices[1]);
VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
VEC_SWAP(tu_vertices[0], tu_vertices[1]);
VEC_SCALE_4(tu_plane, -1.0f, tu_plane);
distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]);
distances[1] = -distances[1];
}
else
{
distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]);
}
}
@@ -448,47 +437,44 @@ public:
}
else
{*/
bl = 0;
if(distances[0]<distances[1]) bl = 1;
bl = 0;
if (distances[0] < distances[1]) bl = 1;
//}
if(bl==2) //edge edge separation
if (bl == 2) //edge edge separation
{
if(distances[2]>margin) return false;
if (distances[2] > margin) return false;
contacts.m_penetration_depth = -distances[2] + margin;
contacts.m_points[0] = closest_point_v;
contacts.m_point_count = 1;
VEC_COPY(contacts.m_separating_normal,edge_edge_dir);
VEC_COPY(contacts.m_separating_normal, edge_edge_dir);
return true;
}
//clip face against other
GUINT point_count;
//TODO
if(bl == 0) //clip U points against V
if (bl == 0) //clip U points against V
{
point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
if(point_count == 0) return false;
contacts.merge_points(tv_plane,margin,contact_points,point_count);
point_count = clip_triangle(tv_plane, tv_vertices, tu_vertices, contact_points);
if (point_count == 0) return false;
contacts.merge_points(tv_plane, margin, contact_points, point_count);
}
else //clip V points against U
else //clip V points against U
{
point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
if(point_count == 0) return false;
contacts.merge_points(tu_plane,margin,contact_points,point_count);
point_count = clip_triangle(tu_plane, tu_vertices, tv_vertices, contact_points);
if (point_count == 0) return false;
contacts.merge_points(tu_plane, margin, contact_points, point_count);
contacts.m_separating_normal *= -1.f;
}
if(contacts.m_point_count == 0) return false;
if (contacts.m_point_count == 0) return false;
return true;
}
};
/*class GIM_TRIANGLE_CALCULATION_CACHE
{
public:
@@ -621,20 +607,13 @@ public:
};*/
bool GIM_TRIANGLE::collide_triangle_hard_test(
const GIM_TRIANGLE & other,
GIM_TRIANGLE_CONTACT_DATA & contact_data) const
const GIM_TRIANGLE &other,
GIM_TRIANGLE_CONTACT_DATA &contact_data) const
{
GIM_TRIANGLE_CALCULATION_CACHE calc_cache;
GIM_TRIANGLE_CALCULATION_CACHE calc_cache;
return calc_cache.triangle_collision(
m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
contact_data);
m_vertices[0], m_vertices[1], m_vertices[2], m_margin,
other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], other.m_margin,
contact_data);
}

260
src/BulletCollision/Gimpact/gim_tri_collision.h
View File

@@ -36,8 +36,6 @@ email: projectileman@yahoo.com
#include "gim_box_collision.h"
#include "gim_clip_polygon.h"
#ifndef MAX_TRI_CLIPPING
#define MAX_TRI_CLIPPING 16
#endif
@@ -45,18 +43,18 @@ email: projectileman@yahoo.com
//! Structure for collision
struct GIM_TRIANGLE_CONTACT_DATA
{
GREAL m_penetration_depth;
GUINT m_point_count;
btVector4 m_separating_normal;
btVector3 m_points[MAX_TRI_CLIPPING];
GREAL m_penetration_depth;
GUINT m_point_count;
btVector4 m_separating_normal;
btVector3 m_points[MAX_TRI_CLIPPING];
SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other)
SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA &other)
{
m_penetration_depth = other.m_penetration_depth;
m_separating_normal = other.m_separating_normal;
m_point_count = other.m_point_count;
GUINT i = m_point_count;
while(i--)
while (i--)
{
m_points[i] = other.m_points[i];
}
@@ -66,39 +64,36 @@ struct GIM_TRIANGLE_CONTACT_DATA
{
}
GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other)
GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA &other)
{
copy_from(other);
}
//! classify points that are closer
template<typename DISTANCE_FUNC,typename CLASS_PLANE>
SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane,
GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func)
{
m_point_count = 0;
m_penetration_depth= -1000.0f;
//! classify points that are closer
template <typename DISTANCE_FUNC, typename CLASS_PLANE>
SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE &plane,
GREAL margin, const btVector3 *points, GUINT point_count, DISTANCE_FUNC distance_func)
{
m_point_count = 0;
m_penetration_depth = -1000.0f;
GUINT point_indices[MAX_TRI_CLIPPING];
GUINT _k;
for(_k=0;_k<point_count;_k++)
for (_k = 0; _k < point_count; _k++)
{
GREAL _dist = -distance_func(plane,points[_k]) + margin;
GREAL _dist = -distance_func(plane, points[_k]) + margin;
if(_dist>=0.0f)
if (_dist >= 0.0f)
{
if(_dist>m_penetration_depth)
if (_dist > m_penetration_depth)
{
m_penetration_depth = _dist;
point_indices[0] = _k;
m_point_count=1;
m_point_count = 1;
}
else if((_dist+G_EPSILON)>=m_penetration_depth)
else if ((_dist + G_EPSILON) >= m_penetration_depth)
{
point_indices[m_point_count] = _k;
m_point_count++;
@@ -106,88 +101,87 @@ struct GIM_TRIANGLE_CONTACT_DATA
}
}
for( _k=0;_k<m_point_count;_k++)
for (_k = 0; _k < m_point_count; _k++)
{
m_points[_k] = points[point_indices[_k]];
}
}
//! classify points that are closer
SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin,
const btVector3 * points, GUINT point_count)
SIMD_FORCE_INLINE void merge_points(const btVector4 &plane, GREAL margin,
const btVector3 *points, GUINT point_count)
{
m_separating_normal = plane;
mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC());
}
};
//! Class for colliding triangles
class GIM_TRIANGLE
{
public:
btScalar m_margin;
btVector3 m_vertices[3];
btVector3 m_vertices[3];
GIM_TRIANGLE():m_margin(0.1f)
{
}
GIM_TRIANGLE() : m_margin(0.1f)
{
}
SIMD_FORCE_INLINE GIM_AABB get_box() const
{
return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
}
SIMD_FORCE_INLINE GIM_AABB get_box() const
{
return GIM_AABB(m_vertices[0], m_vertices[1], m_vertices[2], m_margin);
}
SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const
{
TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal);
}
SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const
{
TRIANGLE_NORMAL(m_vertices[0], m_vertices[1], m_vertices[2], normal);
}
SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const
{
TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);;
}
SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const
{
TRIANGLE_PLANE(m_vertices[0], m_vertices[1], m_vertices[2], plane);
;
}
SIMD_FORCE_INLINE void apply_transform(const btTransform & trans)
{
m_vertices[0] = trans(m_vertices[0]);
m_vertices[1] = trans(m_vertices[1]);
m_vertices[2] = trans(m_vertices[2]);
}
SIMD_FORCE_INLINE void apply_transform(const btTransform &trans)
{
m_vertices[0] = trans(m_vertices[0]);
m_vertices[1] = trans(m_vertices[1]);
m_vertices[2] = trans(m_vertices[2]);
}
SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane) const
{
const btVector3 & e0 = m_vertices[edge_index];
const btVector3 & e1 = m_vertices[(edge_index+1)%3];
EDGE_PLANE(e0,e1,triangle_normal,plane);
}
SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index, const btVector3 &triangle_normal, btVector4 &plane) const
{
const btVector3 &e0 = m_vertices[edge_index];
const btVector3 &e1 = m_vertices[(edge_index + 1) % 3];
EDGE_PLANE(e0, e1, triangle_normal, plane);
}
//! Gets the relative transformation of this triangle
/*!
//! Gets the relative transformation of this triangle
/*!
The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0:
- triangle normal corresponds to Z axis.
- 1st normalized edge corresponds to X axis,
*/
SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform) const
{
btMatrix3x3 & matrix = triangle_transform.getBasis();
SIMD_FORCE_INLINE void get_triangle_transform(btTransform &triangle_transform) const
{
btMatrix3x3 &matrix = triangle_transform.getBasis();
btVector3 zaxis;
get_normal(zaxis);
MAT_SET_Z(matrix,zaxis);
btVector3 zaxis;
get_normal(zaxis);
MAT_SET_Z(matrix, zaxis);
btVector3 xaxis = m_vertices[1] - m_vertices[0];
VEC_NORMALIZE(xaxis);
MAT_SET_X(matrix,xaxis);
btVector3 xaxis = m_vertices[1] - m_vertices[0];
VEC_NORMALIZE(xaxis);
MAT_SET_X(matrix, xaxis);
//y axis
xaxis = zaxis.cross(xaxis);
MAT_SET_Y(matrix,xaxis);
triangle_transform.setOrigin(m_vertices[0]);
}
//y axis
xaxis = zaxis.cross(xaxis);
MAT_SET_Y(matrix, xaxis);
triangle_transform.setOrigin(m_vertices[0]);
}
//! Test triangles by finding separating axis
/*!
@@ -195,8 +189,8 @@ public:
\param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle
*/
bool collide_triangle_hard_test(
const GIM_TRIANGLE & other,
GIM_TRIANGLE_CONTACT_DATA & contact_data) const;
const GIM_TRIANGLE &other,
GIM_TRIANGLE_CONTACT_DATA &contact_data) const;
//! Test boxes before doing hard test
/*!
@@ -205,16 +199,16 @@ public:
\
*/
SIMD_FORCE_INLINE bool collide_triangle(
const GIM_TRIANGLE & other,
GIM_TRIANGLE_CONTACT_DATA & contact_data) const
const GIM_TRIANGLE &other,
GIM_TRIANGLE_CONTACT_DATA &contact_data) const
{
//test box collisioin
GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin);
GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin);
if(!boxu.has_collision(boxv)) return false;
GIM_AABB boxu(m_vertices[0], m_vertices[1], m_vertices[2], m_margin);
GIM_AABB boxv(other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], other.m_margin);
if (!boxu.has_collision(boxv)) return false;
//do hard test
return collide_triangle_hard_test(other,contact_data);
return collide_triangle_hard_test(other, contact_data);
}
/*!
@@ -246,43 +240,43 @@ if 0.0<= u+v <=1.0 then they are inside of triangle
\return false if the point is outside of triangle.This function doesn't take the margin
*/
SIMD_FORCE_INLINE bool get_uv_parameters(
const btVector3 & point,
const btVector3 & tri_plane,
GREAL & u, GREAL & v) const
const btVector3 &point,
const btVector3 &tri_plane,
GREAL &u, GREAL &v) const
{
btVector3 _axe1 = m_vertices[1]-m_vertices[0];
btVector3 _axe2 = m_vertices[2]-m_vertices[0];
btVector3 _axe1 = m_vertices[1] - m_vertices[0];
btVector3 _axe2 = m_vertices[2] - m_vertices[0];
btVector3 _vecproj = point - m_vertices[0];
GUINT _i1 = (tri_plane.closestAxis()+1)%3;
GUINT _i2 = (_i1+1)%3;
if(btFabs(_axe2[_i2])<G_EPSILON)
GUINT _i1 = (tri_plane.closestAxis() + 1) % 3;
GUINT _i2 = (_i1 + 1) % 3;
if (btFabs(_axe2[_i2]) < G_EPSILON)
{
u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1] - _axe1[_i1]*_axe2[_i2]);
v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1];
u = (_vecproj[_i2] * _axe2[_i1] - _vecproj[_i1] * _axe2[_i2]) / (_axe1[_i2] * _axe2[_i1] - _axe1[_i1] * _axe2[_i2]);
v = (_vecproj[_i1] - u * _axe1[_i1]) / _axe2[_i1];
}
else
{
u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2] - _axe1[_i2]*_axe2[_i1]);
v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2];
u = (_vecproj[_i1] * _axe2[_i2] - _vecproj[_i2] * _axe2[_i1]) / (_axe1[_i1] * _axe2[_i2] - _axe1[_i2] * _axe2[_i1]);
v = (_vecproj[_i2] - u * _axe1[_i2]) / _axe2[_i2];
}
if(u<-G_EPSILON)
if (u < -G_EPSILON)
{
return false;
}
else if(v<-G_EPSILON)
else if (v < -G_EPSILON)
{
return false;
}
else
{
btScalar sumuv;
sumuv = u+v;
if(sumuv<-G_EPSILON)
sumuv = u + v;
if (sumuv < -G_EPSILON)
{
return false;
}
else if(sumuv-1.0f>G_EPSILON)
else if (sumuv - 1.0f > G_EPSILON)
{
return false;
}
@@ -294,50 +288,49 @@ if 0.0<= u+v <=1.0 then they are inside of triangle
/*!
Test if point is in triangle, with m_margin tolerance
*/
SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const
SIMD_FORCE_INLINE bool is_point_inside(const btVector3 &point, const btVector3 &tri_normal) const
{
//Test with edge 0
btVector4 edge_plane;
this->get_edge_plane(0,tri_normal,edge_plane);
GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point);
if(dist-m_margin>0.0f) return false; // outside plane
this->get_edge_plane(0, tri_normal, edge_plane);
GREAL dist = DISTANCE_PLANE_POINT(edge_plane, point);
if (dist - m_margin > 0.0f) return false; // outside plane
this->get_edge_plane(1,tri_normal,edge_plane);
dist = DISTANCE_PLANE_POINT(edge_plane,point);
if(dist-m_margin>0.0f) return false; // outside plane
this->get_edge_plane(1, tri_normal, edge_plane);
dist = DISTANCE_PLANE_POINT(edge_plane, point);
if (dist - m_margin > 0.0f) return false; // outside plane
this->get_edge_plane(2,tri_normal,edge_plane);
dist = DISTANCE_PLANE_POINT(edge_plane,point);
if(dist-m_margin>0.0f) return false; // outside plane
this->get_edge_plane(2, tri_normal, edge_plane);
dist = DISTANCE_PLANE_POINT(edge_plane, point);
if (dist - m_margin > 0.0f) return false; // outside plane
return true;
}
//! Bidireccional ray collision
SIMD_FORCE_INLINE bool ray_collision(
const btVector3 & vPoint,
const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
const btVector3 &vPoint,
const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal,
GREAL &tparam, GREAL tmax = G_REAL_INFINITY)
{
btVector4 faceplane;
{
btVector3 dif1 = m_vertices[1] - m_vertices[0];
btVector3 dif2 = m_vertices[2] - m_vertices[0];
VEC_CROSS(faceplane,dif1,dif2);
faceplane[3] = m_vertices[0].dot(faceplane);
VEC_CROSS(faceplane, dif1, dif2);
faceplane[3] = m_vertices[0].dot(faceplane);
}
GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
if(res == 0) return false;
if(! is_point_inside(pout,faceplane)) return false;
GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax);
if (res == 0) return false;
if (!is_point_inside(pout, faceplane)) return false;
if(res==2) //invert normal
if (res == 2) //invert normal
{
triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]);
triangle_normal.setValue(-faceplane[0], -faceplane[1], -faceplane[2]);
}
else
{
triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]);
}
VEC_NORMALIZE(triangle_normal);
@@ -345,36 +338,31 @@ if 0.0<= u+v <=1.0 then they are inside of triangle
return true;
}
//! one direccion ray collision
SIMD_FORCE_INLINE bool ray_collision_front_side(
const btVector3 & vPoint,
const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal,
GREAL & tparam, GREAL tmax = G_REAL_INFINITY)
const btVector3 &vPoint,
const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal,
GREAL &tparam, GREAL tmax = G_REAL_INFINITY)
{
btVector4 faceplane;
{
btVector3 dif1 = m_vertices[1] - m_vertices[0];
btVector3 dif2 = m_vertices[2] - m_vertices[0];
VEC_CROSS(faceplane,dif1,dif2);
faceplane[3] = m_vertices[0].dot(faceplane);
VEC_CROSS(faceplane, dif1, dif2);
faceplane[3] = m_vertices[0].dot(faceplane);
}
GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax);
if(res != 1) return false;
GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax);
if (res != 1) return false;
if(!is_point_inside(pout,faceplane)) return false;
if (!is_point_inside(pout, faceplane)) return false;
triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]);
triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]);
VEC_NORMALIZE(triangle_normal);
return true;
}
};
#endif // GIM_TRI_COLLISION_H_INCLUDED
#endif // GIM_TRI_COLLISION_H_INCLUDED