diff --git a/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp b/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp index 4126d03ed0..73ebcdac98 100644 --- a/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp +++ b/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp @@ -1,6 +1,7 @@ bool searchIncremental3dSapOnGpu = true; #include +#include #include "b3GpuSapBroadphase.h" #include "Bullet3Common/b3Vector3.h" #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" @@ -183,7 +184,8 @@ static bool TestAabbAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aab //http://stereopsis.com/radix.html static unsigned int FloatFlip(float fl) { - unsigned int f = *(unsigned int*)&fl; + unsigned int f; + memcpy(&f, &fl, sizeof(f)); unsigned int mask = -(int)(f >> 31) | 0x80000000; return f ^ mask; }; diff --git a/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp b/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp index e86af6583f..c39ff3b2cb 100644 --- a/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp +++ b/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp @@ -202,7 +202,7 @@ void b3RadixSort32CL::execute(b3OpenCLArray& keyValuesInOut, int sor #define USE_BTFILL #ifdef USE_BTFILL - m_fill->execute((b3OpenCLArray&)*m_workBuffer4, (b3Int2&)fillValue, workingSize - originalSize, originalSize); + m_fill->execute((b3OpenCLArray&)*m_workBuffer4, b3MakeInt2(fillValue.m_key, fillValue.m_value), workingSize - originalSize, originalSize); #else //fill the remaining bits (very slow way, todo: fill on GPU/OpenCL side) diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp index e3d235a4fd..873cb76b2e 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp @@ -242,16 +242,9 @@ b3GpuPgsContactSolver::~b3GpuPgsContactSolver() delete m_data; } -struct b3ConstraintCfg +struct b3ConstraintCfg : public b3SolverBase::ConstraintCfg { - b3ConstraintCfg(float dt = 0.f) : m_positionDrift(0.005f), m_positionConstraintCoeff(0.2f), m_dt(dt), m_staticIdx(0) {} - - float m_positionDrift; - float m_positionConstraintCoeff; - float m_dt; - bool m_enableParallelSolve; - float m_batchCellSize; - int m_staticIdx; + b3ConstraintCfg(float dt = 0.f) : b3SolverBase::ConstraintCfg(dt) {m_staticIdx = 0;} }; void b3GpuPgsContactSolver::solveContactConstraintBatchSizes(const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, @@ -1040,7 +1033,7 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem shapeBuf, m_data->m_solverGPU->m_contactBuffer2, contactConstraintOut, additionalData, nContacts, - (b3SolverBase::ConstraintCfg&)csCfg); + csCfg); clFinish(m_data->m_queue); } diff --git a/src/BulletCollision/Gimpact/btContactProcessingStructs.h b/src/BulletCollision/Gimpact/btContactProcessingStructs.h index bc8a709246..f5a1654d33 100644 --- a/src/BulletCollision/Gimpact/btContactProcessingStructs.h +++ b/src/BulletCollision/Gimpact/btContactProcessingStructs.h @@ -24,9 +24,7 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#include "LinearMath/btTransform.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "btTriangleShapeEx.h" +#include "LinearMath/btVector3.h" /** Configuration var for applying interpolation of contact normals @@ -37,6 +35,12 @@ Configuration var for applying interpolation of contact normals ///The GIM_CONTACT is an internal GIMPACT structure, similar to btManifoldPoint. ///@todo: remove and replace GIM_CONTACT by btManifoldPoint. + +/// Structure for collision results +///Functions for managing and sorting contacts resulting from a collision query. +///Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST +///After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY +///Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts class GIM_CONTACT { public: diff --git a/src/BulletCollision/Gimpact/gim_contact.h b/src/BulletCollision/Gimpact/gim_contact.h index 9deb28a26e..fe7382cb9f 100644 --- a/src/BulletCollision/Gimpact/gim_contact.h +++ b/src/BulletCollision/Gimpact/gim_contact.h @@ -36,95 +36,7 @@ email: projectileman@yahoo.com #include "gim_radixsort.h" #include "gim_array.h" -/** -Configuration var for applying interpolation of contact normals -*/ -#ifndef NORMAL_CONTACT_AVERAGE -#define NORMAL_CONTACT_AVERAGE 1 -#endif - -#ifndef CONTACT_DIFF_EPSILON -#define CONTACT_DIFF_EPSILON 0.00001f -#endif - -#ifndef BT_CONTACT_H_STRUCTS_INCLUDED - -/// Structure for collision results -///Functions for managing and sorting contacts resulting from a collision query. -///Contact lists must be create by calling \ref GIM_CREATE_CONTACT_LIST -///After querys, contact lists must be destroy by calling \ref GIM_DYNARRAY_DESTROY -///Contacts can be merge for avoid duplicate results by calling \ref gim_merge_contacts -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 -public: - GIM_CONTACT() - { - } - - GIM_CONTACT(const GIM_CONTACT &contact) : m_point(contact.m_point), - m_normal(contact.m_normal), - m_depth(contact.m_depth), - m_feature1(contact.m_feature1), - m_feature2(contact.m_feature2) - { - m_point = contact.m_point; - m_normal = contact.m_normal; - m_depth = contact.m_depth; - m_feature1 = contact.m_feature1; - m_feature2 = contact.m_feature2; - } - - GIM_CONTACT(const btVector3 &point, const btVector3 &normal, - GREAL depth, GUINT feature1, GUINT feature2) : m_point(point), - m_normal(normal), - m_depth(depth), - m_feature1(feature1), - m_feature2(feature2) - { - } - - //! 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; - GUINT *_uitmp = (GUINT *)(&_coords[0]); - _hash = *_uitmp; - _uitmp++; - _hash += (*_uitmp) << 4; - _uitmp++; - _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++) - { - vec_sum += normals[i]; - } - - GREAL vec_sum_len = vec_sum.length2(); - if (vec_sum_len < CONTACT_DIFF_EPSILON) return; - - GIM_INV_SQRT(vec_sum_len, vec_sum_len); // 1/sqrt(vec_sum_len) - - m_normal = vec_sum * vec_sum_len; - } -}; - -#endif +#include "btContactProcessingStructs.h" class gim_contact_array : public gim_array { diff --git a/src/BulletCollision/Gimpact/gim_math.h b/src/BulletCollision/Gimpact/gim_math.h index 3c4f821a72..33f9aa336e 100644 --- a/src/BulletCollision/Gimpact/gim_math.h +++ b/src/BulletCollision/Gimpact/gim_math.h @@ -32,6 +32,7 @@ email: projectileman@yahoo.com ----------------------------------------------------------------------------- */ +#include #include "LinearMath/btScalar.h" #define GREAL btScalar @@ -70,17 +71,31 @@ enum GIM_SCALAR_TYPES #define G_DEGTORAD(X) ((X)*3.1415926f / 180.0f) #define G_RADTODEG(X) ((X)*180.0f / 3.1415926f) +static GUINT gim_ir__(float r) +{ + GUINT i; + memcpy(&i, &r, sizeof(i)); + return i; +} + +static GREAL gim_fr__(GUINT i) +{ + float r; + memcpy(&r, &i, sizeof(r)); + return r; +} + //! Integer representation of a floating-point value. -#define GIM_IR(x) ((GUINT&)(x)) +#define GIM_IR(x) (gim_ir__(x)) //! Signed integer representation of a floating-point value. -#define GIM_SIR(x) ((GINT&)(x)) +#define GIM_SIR(x) ((GINT)gim_ir__(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) (gim_fr__(x)) #define GIM_MAX(a, b) (a < b ? b : a) #define GIM_MIN(a, b) (a > b ? b : a) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c873099b49..119b2fb6c2 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2423,7 +2423,7 @@ void btSoftBody::pointersToIndices() { if (m_nodes[i].m_leaf) { - m_nodes[i].m_leaf->data = *(void**)&i; + m_nodes[i].m_leaf->dataAsInt = i; } } for (i = 0, ni = m_links.size(); i < ni; ++i) @@ -2438,7 +2438,7 @@ void btSoftBody::pointersToIndices() m_faces[i].m_n[2] = PTR2IDX(m_faces[i].m_n[2], base); if (m_faces[i].m_leaf) { - m_faces[i].m_leaf->data = *(void**)&i; + m_faces[i].m_leaf->dataAsInt = i; } } for (i = 0, ni = m_anchors.size(); i < ni; ++i) diff --git a/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp index dcdd932028..b38fce4724 100644 --- a/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp @@ -177,6 +177,7 @@ void btSoftMultiBodyDynamicsWorld::debugDrawWorld() } } +namespace { struct btSoftSingleRayCallback : public btBroadphaseRayCallback { btVector3 m_rayFromWorld; @@ -250,6 +251,7 @@ struct btSoftSingleRayCallback : public btBroadphaseRayCallback return true; } }; +} void btSoftMultiBodyDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const { diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index dc9b004a37..b3d8486c78 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -172,6 +172,7 @@ void btSoftRigidDynamicsWorld::debugDrawWorld() } } +namespace { struct btSoftSingleRayCallback : public btBroadphaseRayCallback { btVector3 m_rayFromWorld; @@ -245,6 +246,7 @@ struct btSoftSingleRayCallback : public btBroadphaseRayCallback return true; } }; +} void btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const {