diff --git a/Demos/BasicDemo/BasicDemo.cpp b/Demos/BasicDemo/BasicDemo.cpp index 9620a4cd0..393797da8 100644 --- a/Demos/BasicDemo/BasicDemo.cpp +++ b/Demos/BasicDemo/BasicDemo.cpp @@ -29,13 +29,33 @@ float gCollisionMargin = 0.05f; #include "BasicDemo.h" #include "GL_ShapeDrawer.h" #include "GlutStuff.h" + +#include + //////////////////////////////////// GLDebugDrawer debugDrawer; +class myTest +{ + +}; + + + int main(int argc,char** argv) { + { + ///btAlignedObjectArray works the same as std::vector but + ///allows 16-byte aligned objects like SIMD vectors etc. + btAlignedObjectArray m_points; + m_points.push_back(btVector3(1,2,3)); + const btVector3& ref = m_points[0]; + m_points[0] = btVector3(2,3,4); + m_points.pop_back(); + } + BasicDemo ccdDemo; ccdDemo.initPhysics(); ccdDemo.setCameraDistance(50.f); diff --git a/Demos/BspDemo/BspConverter.cpp b/Demos/BspDemo/BspConverter.cpp index aa3530e25..6cb6255a0 100644 --- a/Demos/BspDemo/BspConverter.cpp +++ b/Demos/BspDemo/BspConverter.cpp @@ -53,7 +53,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling) for (int b=0;b planeEquations; + btAlignedObjectArray planeEquations; int brushid = bspLoader.m_dleafbrushes[leaf.firstLeafBrush+b]; @@ -83,7 +83,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling) if (isValidBrush) { - std::vector vertices; + btAlignedObjectArray vertices; btGeometryUtil::getVerticesFromPlaneEquations(planeEquations,vertices); bool isEntity = false; @@ -134,7 +134,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling) const BSPModel& model = bspLoader.m_dmodels[modelnr]; for (int n=0;n planeEquations; + btAlignedObjectArray planeEquations; bool isValidBrush = false; //convert brush @@ -158,7 +158,7 @@ void BspConverter::convertBsp(BspLoader& bspLoader,float scaling) if (isValidBrush) { - std::vector vertices; + btAlignedObjectArray vertices; btGeometryUtil::getVerticesFromPlaneEquations(planeEquations,vertices); bool isEntity=true; diff --git a/Demos/BspDemo/BspConverter.h b/Demos/BspDemo/BspConverter.h index e841c1f2d..743d46da9 100644 --- a/Demos/BspDemo/BspConverter.h +++ b/Demos/BspDemo/BspConverter.h @@ -19,6 +19,7 @@ subject to the following restrictions: class BspLoader; #include #include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" ///BspConverter turns a loaded bsp level into convex parts (vertices) class BspConverter @@ -31,7 +32,7 @@ class BspConverter } ///this callback is called for each brush that succesfully converted into vertices - virtual void addConvexVerticesCollider(std::vector& vertices, bool isEntity, const btVector3& entityTargetLocation) = 0; + virtual void addConvexVerticesCollider(btAlignedObjectArray& vertices, bool isEntity, const btVector3& entityTargetLocation) = 0; }; diff --git a/Demos/BspDemo/BspDemo.cpp b/Demos/BspDemo/BspDemo.cpp index a0d5a7b77..7a9490679 100644 --- a/Demos/BspDemo/BspDemo.cpp +++ b/Demos/BspDemo/BspDemo.cpp @@ -56,7 +56,7 @@ public: { } - virtual void addConvexVerticesCollider(std::vector& vertices, bool isEntity, const btVector3& entityTargetLocation) + virtual void addConvexVerticesCollider(btAlignedObjectArray& vertices, bool isEntity, const btVector3& entityTargetLocation) { ///perhaps we can do something special with entities (isEntity) ///like adding a collision Triggering (as example) diff --git a/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp b/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp index e8f6d2512..2c0d317e5 100644 --- a/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp +++ b/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp @@ -91,7 +91,7 @@ void CollisionInterfaceDemo::initPhysics() //SimpleBroadphase* broadphase = new btSimpleBroadphase; collisionWorld = new btCollisionWorld(dispatcher,broadphase); - + collisionWorld->addCollisionObject(&objects[0]); collisionWorld->addCollisionObject(&objects[1]); @@ -117,12 +117,10 @@ void CollisionInterfaceDemo::displayCallback(void) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); - - btDispatcherInfo dispatchInfo; - dispatchInfo.m_debugDraw = &debugDrawer; + collisionWorld->getDispatchInfo().m_debugDraw = &debugDrawer; if (collisionWorld) - collisionWorld->performDiscreteCollisionDetection(dispatchInfo); + collisionWorld->performDiscreteCollisionDetection(); int i; diff --git a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp index 0bceeb51f..03d94d150 100644 --- a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp +++ b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp @@ -135,7 +135,7 @@ void ConvexDecompositionDemo::initPhysics(const char* filename) //calc centroid, to shift vertices around center of mass centroid.setValue(0,0,0); - std::vector vertices; + btAlignedObjectArray vertices; if ( 1 ) { //const unsigned int *src = result.mHullIndices; diff --git a/Extras/SATConvexCollision/Geometry.inl b/Extras/SATConvexCollision/Geometry.inl index 21510f819..703a3c51e 100644 --- a/Extras/SATConvexCollision/Geometry.inl +++ b/Extras/SATConvexCollision/Geometry.inl @@ -199,7 +199,7 @@ inline const Vector3 Plane::GetNormal() const return Vector3(base); } -inline const Scalar Plane::GetDistance() const +inline const Scalar Plane::getDistance() const { return GetW(); } diff --git a/Extras/SATConvexCollision/Hull.inl b/Extras/SATConvexCollision/Hull.inl index 912ec1774..8cf62bdf5 100644 --- a/Extras/SATConvexCollision/Hull.inl +++ b/Extras/SATConvexCollision/Hull.inl @@ -19,7 +19,7 @@ #include -inline short Hull::GetNumVertices() const +inline short Hull::getNumVertices() const { return m_numVerts; } @@ -29,12 +29,12 @@ inline short Hull::GetNumFaces() const return m_numFaces; } -inline short Hull::GetNumEdges() const +inline short Hull::getNumEdges() const { return m_numEdges; } -inline const Point3& Hull::GetVertex(short index) const +inline const Point3& Hull::getVertex(short index) const { return m_pVerts[index]; } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 9df6d7186..fa1561973 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -121,8 +121,10 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho -void btCollisionWorld::performDiscreteCollisionDetection(btDispatcherInfo& dispatchInfo) +void btCollisionWorld::performDiscreteCollisionDetection() { + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + BEGIN_PROFILE("performDiscreteCollisionDetection"); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 9c5487848..bd09d8c4d 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -173,7 +173,7 @@ public: struct ClosestRayResultCallback : public RayResultCallback { - ClosestRayResultCallback(btVector3 rayFromWorld,btVector3 rayToWorld) + ClosestRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) :m_rayFromWorld(rayFromWorld), m_rayToWorld(rayToWorld), m_collisionObject(0) @@ -237,7 +237,7 @@ public: void removeCollisionObject(btCollisionObject* collisionObject); - virtual void performDiscreteCollisionDetection( btDispatcherInfo& dispatchInfo); + virtual void performDiscreteCollisionDetection(); btDispatcherInfo& getDispatchInfo() { diff --git a/src/BulletCollision/CollisionShapes/btCompoundShape.h b/src/BulletCollision/CollisionShapes/btCompoundShape.h index c810a6548..84188bc8b 100644 --- a/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -23,6 +23,7 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" #include #include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "LinearMath/btAlignedObjectArray.h" class btOptimizedBvh; @@ -30,8 +31,8 @@ class btOptimizedBvh; /// This allows for concave collision objects. This is more general then the Static Concave btTriangleMeshShape. class btCompoundShape : public btCollisionShape { - std::vector m_childTransforms; - std::vector m_childShapes; + btAlignedObjectArray m_childTransforms; + btAlignedObjectArray m_childShapes; btVector3 m_localAabbMin; btVector3 m_localAabbMax; diff --git a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 7a4c7ebf5..5af6e5f03 100644 --- a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -22,6 +22,7 @@ subject to the following restrictions: btConvexHullShape ::btConvexHullShape (const float* points,int numPoints,int stride) { m_points.resize(numPoints); + unsigned char* pointsBaseAddress = (unsigned char*)points; for (int i=0;i + +#include "LinearMath/btAlignedObjectArray.h" ///ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices) ///No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices. @@ -27,7 +28,7 @@ subject to the following restrictions: ///(memory is much slower then the cpu) class btConvexHullShape : public btPolyhedralConvexShape { - std::vector m_points; + btAlignedObjectArray m_points; public: ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive float (x,y,z), the striding defines the number of bytes between each point, in memory. diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 18b796b39..37f15e1dc 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -18,6 +18,11 @@ subject to the following restrictions: #include "LinearMath/btAabbUtil2.h" +btOptimizedBvh::btOptimizedBvh() :m_rootNode1(0), m_numNodes(0) +{ + +} + void btOptimizedBvh::build(btStridingMeshInterface* triangles) { diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index 96172c4e2..cb76cb233 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -15,9 +15,17 @@ subject to the following restrictions: #ifndef OPTIMIZED_BVH_H #define OPTIMIZED_BVH_H + + #include "LinearMath/btVector3.h" + + +//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp + + #include + class btStridingMeshInterface; /// btOptimizedBvhNode contains both internal and leaf node information. @@ -26,7 +34,7 @@ class btStridingMeshInterface; /// and storing aabbmin/max as quantized integers. /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans -struct btOptimizedBvhNode +ATTRIBUTE_ALIGNED16 (struct btOptimizedBvhNode) { btVector3 m_aabbMin; @@ -52,12 +60,23 @@ public: virtual void processNode(const btOptimizedBvhNode* node) = 0; }; -typedef std::vector NodeArray; +#include "LinearMath/btAlignedAllocator.h" +#include "LinearMath/btAlignedObjectArray.h" + +//typedef std::vector< unsigned , allocator_type > container_type; +const unsigned size = (1 << 20); +typedef btAlignedAllocator< btOptimizedBvhNode , size > allocator_type; + +//typedef btAlignedObjectArray NodeArray; + +typedef btAlignedObjectArray NodeArray; ///OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) class btOptimizedBvh { + NodeArray m_leafNodes; + btOptimizedBvhNode* m_rootNode1; btOptimizedBvhNode* m_contiguousNodes; @@ -65,10 +84,11 @@ class btOptimizedBvh int m_numNodes; - NodeArray m_leafNodes; + public: - btOptimizedBvh() :m_rootNode1(0), m_numNodes(0) { } + btOptimizedBvh(); + virtual ~btOptimizedBvh(); void build(btStridingMeshInterface* triangles); diff --git a/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index ddd66cc23..d4fd0e330 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -17,7 +17,8 @@ subject to the following restrictions: #define BT_TRIANGLE_BUFFER_H #include "btTriangleCallback.h" -#include +//#include +#include "LinearMath/btAlignedObjectArray.h" struct btTriangle { @@ -32,7 +33,7 @@ struct btTriangle class btTriangleBuffer : public btTriangleCallback { - std::vector m_triangleBuffer; + btAlignedObjectArray m_triangleBuffer; public: diff --git a/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index 638c8b87f..3ec827c03 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define BT_TRIANGLE_INDEX_VERTEX_ARRAY_H #include "btStridingMeshInterface.h" -#include +#include ///IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements ///instead of the number of indices, we pass the number of triangles @@ -38,7 +38,7 @@ struct btIndexedMesh ///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray. class btTriangleIndexVertexArray : public btStridingMeshInterface { - std::vector m_indexedMeshes; + btAlignedObjectArray m_indexedMeshes; public: diff --git a/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/src/BulletCollision/CollisionShapes/btTriangleMesh.h index 171dcf33b..1be03d704 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -18,9 +18,8 @@ subject to the following restrictions: #define TRIANGLE_MESH_H #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" -#include #include - +#include "LinearMath/btAlignedObjectArray.h" struct btMyTriangle { btVector3 m_vert0; @@ -31,8 +30,7 @@ struct btMyTriangle ///TriangleMesh provides storage for a concave triangle mesh. It can be used as data for the btTriangleMeshShape. class btTriangleMesh : public btStridingMeshInterface { - std::vector m_triangles; - + btAlignedObjectArray m_triangles; public: btTriangleMesh (); diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index fe61d2cee..3d5db5173 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -261,7 +261,7 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep) dispatchInfo.m_debugDraw = getDebugDrawer(); ///perform collision detection - performDiscreteCollisionDetection(dispatchInfo); + performDiscreteCollisionDetection(); calculateSimulationIslands(); diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index 453b8ec42..705c023d3 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -110,16 +110,17 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi #endif //OBSOLETE_MOTIONSTATE_LESS -#define EXPERIMENTAL_JITTER_REMOVAL 1 +//#define EXPERIMENTAL_JITTER_REMOVAL 1 #ifdef EXPERIMENTAL_JITTER_REMOVAL //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate //doesn't work very well yet (value 0 disabled this damping) //note there this influences deactivation thresholds! float gClippedAngvelThresholdSqr = 0.01f; float gClippedLinearThresholdSqr = 0.01f; -float gJitterVelocityDampingFactor = 1.f; #endif //EXPERIMENTAL_JITTER_REMOVAL +float gJitterVelocityDampingFactor = 1.f; + void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) { diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 2a637b651..c129dc7e7 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -46,13 +46,13 @@ int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa ///apply gravity, predict motion predictUnconstraintMotion(timeStep); - btDispatcherInfo dispatchInfo; + btDispatcherInfo& dispatchInfo = getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; dispatchInfo.m_debugDraw = getDebugDrawer(); ///perform collision detection - performDiscreteCollisionDetection(dispatchInfo ); + performDiscreteCollisionDetection(); ///solve contact constraints int numManifolds = m_dispatcher1->getNumManifolds(); diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index d4dcf1a45..1be7aee50 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -128,11 +128,10 @@ void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT void btRaycastVehicle::resetSuspension() { - std::vector::iterator wheelIt; - for (wheelIt = m_wheelInfo.begin(); - !(wheelIt == m_wheelInfo.end());wheelIt++) + int i; + for (i=0;i::iterator wheelIt; + int i=0; - for (wheelIt = m_wheelInfo.begin(); - !(wheelIt == m_wheelInfo.end());wheelIt++,i++) + for (i=0;igetCenterOfMassPosition(); btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos ); diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/src/BulletDynamics/Vehicle/btRaycastVehicle.h index 9a1a30ff7..9fd332f43 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -15,7 +15,7 @@ #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" #include "btVehicleRaycaster.h" class btDynamicsWorld; - +#include "LinearMath/btAlignedObjectArray.h" #include "btWheelInfo.h" class btVehicleTuning; @@ -95,7 +95,7 @@ public: return int (m_wheelInfo.size()); } - std::vector m_wheelInfo; + btAlignedObjectArray m_wheelInfo; const btWheelInfo& getWheelInfo(int index) const; diff --git a/src/LinearMath/btAlignedAllocator.h b/src/LinearMath/btAlignedAllocator.h new file mode 100644 index 000000000..9a334eab1 --- /dev/null +++ b/src/LinearMath/btAlignedAllocator.h @@ -0,0 +1,71 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_ALIGNED_ALLOCATOR +#define BT_ALIGNED_ALLOCATOR + +///we probably replace this with our own aligned memory allocator +///so we replace _aligned_malloc and _aligned_free with our own +///that is better portable and more predictable +#include + +typedef int size_type; + + +template < typename T , unsigned Alignment > +class btAlignedAllocator { + + typedef btAlignedAllocator< T , Alignment > self_type; + +public: + //just going down a list: + btAlignedAllocator() {} + + btAlignedAllocator( const self_type & ) {} + + template < typename Other > + btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {} + + typedef const T* const_pointer; + typedef const T& const_reference; + typedef T* pointer; + typedef T& reference; + typedef T value_type; + + pointer address ( reference ref ) const { return &ref; } + const_pointer address ( const_reference ref ) const { return &ref; } + pointer allocate ( size_type n , const_pointer * hint = 0 ) { + return reinterpret_cast< pointer >(_aligned_malloc( sizeof(value_type) * n , Alignment )); + } + void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); } + void deallocate( pointer ptr ) { + _aligned_free( reinterpret_cast< void * >( ptr ) ); + } + void destroy ( pointer ptr ) { ptr->~value_type(); } + + + template < typename O > struct rebind { + typedef btAlignedAllocator< O , Alignment > other; + }; + template < typename O > + self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; } + + friend bool operator==( const self_type & , const self_type & ) { return true; } +}; + + + +#endif //BT_ALIGNED_ALLOCATOR + diff --git a/src/LinearMath/btAlignedObjectArray.h b/src/LinearMath/btAlignedObjectArray.h new file mode 100644 index 000000000..a5a6ec6a2 --- /dev/null +++ b/src/LinearMath/btAlignedObjectArray.h @@ -0,0 +1,175 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef BT_OBJECT_ARRAY__ +#define BT_OBJECT_ARRAY__ + +#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE +#include "btAlignedAllocator.h" + +///btAlignedObjectArray uses a subset of the stl::vector interface for its methods +///It is developed to replace stl::vector to avoid STL alignment issues to add SIMD/SSE data +template +//template +class btAlignedObjectArray +{ + int m_size; + int m_capacity; + T* m_data; + + btAlignedAllocator m_allocator; + + protected: + SIMD_FORCE_INLINE int allocSize(int size) + { + return (size ? size*2 : 1); + } + SIMD_FORCE_INLINE void copy(int start,int end, T* dest) + { + int i; + for (i=0;i size()) + { + reserve(newsize); + } + + m_size = newsize; + } + + + + SIMD_FORCE_INLINE void push_back(const T& _Val) + { + int sz = size(); + if( sz == capacity() ) + { + reserve( allocSize(size()) ); + } + + m_data[size()] = _Val; + //::new ( m_data[m_size] ) T(_Val); + m_size++; + } + + + + SIMD_FORCE_INLINE void reserve(int _Count) + { // determine new minimum length of allocated storage + if (capacity() < _Count) + { // not enough room, reallocate + if (capacity() < _Count) + { + T* s = (T*)allocate(_Count); + + copy(0, size(), s); + + destroy(0,size()); + + deallocate(); + + m_data = s; + + m_capacity = _Count; + + } + } + } + +}; + +#endif //BT_OBJECT_ARRAY__ \ No newline at end of file diff --git a/src/LinearMath/btGeometryUtil.cpp b/src/LinearMath/btGeometryUtil.cpp index 80264f9dd..ef3798b4c 100644 --- a/src/LinearMath/btGeometryUtil.cpp +++ b/src/LinearMath/btGeometryUtil.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btGeometryUtil.h" -bool btGeometryUtil::isPointInsidePlanes(const std::vector& planeEquations, const btVector3& point, float margin) +bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, float margin) { int numbrushes = planeEquations.size(); for (int i=0;i& planeEqua } -bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const std::vector& vertices, float margin) +bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, float margin) { int numvertices = vertices.size(); for (int i=0;i& planeEquations) +bool notExist(const btVector3& planeEquation,const btAlignedObjectArray& planeEquations) { int numbrushes = planeEquations.size(); for (int i=0;i& plane return true; } -void btGeometryUtil::getPlaneEquationsFromVertices(std::vector& vertices, std::vector& planeEquationsOut ) +void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray& vertices, btAlignedObjectArray& planeEquationsOut ) { const int numvertices = vertices.size(); // brute force: @@ -110,7 +110,7 @@ void btGeometryUtil::getPlaneEquationsFromVertices(std::vector& verti } -void btGeometryUtil::getVerticesFromPlaneEquations(const std::vector& planeEquations , std::vector& verticesOut ) +void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations , btAlignedObjectArray& verticesOut ) { const int numbrushes = planeEquations.size(); // brute force: diff --git a/src/LinearMath/btGeometryUtil.h b/src/LinearMath/btGeometryUtil.h index 714cd7a2e..f3247d21a 100644 --- a/src/LinearMath/btGeometryUtil.h +++ b/src/LinearMath/btGeometryUtil.h @@ -15,23 +15,24 @@ subject to the following restrictions: #ifndef BT_GEOMETRY_UTIL_H #define BT_GEOMETRY_UTIL_H -#include + #include "btVector3.h" +#include "btAlignedObjectArray.h" class btGeometryUtil { public: - static void getPlaneEquationsFromVertices(std::vector& vertices, std::vector& planeEquationsOut ); + static void getPlaneEquationsFromVertices(btAlignedObjectArray& vertices, btAlignedObjectArray& planeEquationsOut ); - static void getVerticesFromPlaneEquations(const std::vector& planeEquations , std::vector& verticesOut ); + static void getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations , btAlignedObjectArray& verticesOut ); - static bool isInside(const std::vector& vertices, const btVector3& planeNormal, float margin); + static bool isInside(const btAlignedObjectArray& vertices, const btVector3& planeNormal, float margin); - static bool isPointInsidePlanes(const std::vector& planeEquations, const btVector3& point, float margin); + static bool isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, float margin); - static bool areVerticesBehindPlane(const btVector3& planeNormal, const std::vector& vertices, float margin); + static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, float margin); }; diff --git a/src/LinearMath/btQuadWord.h b/src/LinearMath/btQuadWord.h index 28bb2051d..4331b6682 100644 --- a/src/LinearMath/btQuadWord.h +++ b/src/LinearMath/btQuadWord.h @@ -21,11 +21,10 @@ subject to the following restrictions: - -class btQuadWord +ATTRIBUTE_ALIGNED16 (class btQuadWord) { protected: - ATTRIBUTE_ALIGNED16 (btScalar m_x); + btScalar m_x; btScalar m_y; btScalar m_z; btScalar m_unusedW; diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 46b8145a7..82394110f 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -34,8 +34,8 @@ subject to the following restrictions: #define SIMD_FORCE_INLINE __forceinline #endif //__MINGW32__ - //#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a - #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + //#define ATTRIBUTE_ALIGNED16(a) a #include #define btAssert assert #else