Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/extern
diff options
context:
space:
mode:
authorErwin Coumans <blender@erwincoumans.com>2005-12-31 10:20:08 +0300
committerErwin Coumans <blender@erwincoumans.com>2005-12-31 10:20:08 +0300
commit9119b6e8a547303ce9a7ccd3a00636153b53cb0e (patch)
tree1816973e2baf4888d571e87cfb548cafafe46d41 /extern
parent625c553e2077ec0a252ddd934d4267c61011d61f (diff)
Fixed several bugs: python refcounting related and Bullet related (basic add/remove object support, bounding volume hierarchy). Added a few files, updated the Bullet scons. Vc6/7 Bullet projectfiles need to add a couple of files: 'Bullet/CollisionShapes/BvhTriangleMeshShape.cpp',
'Bullet/CollisionShapes/ConvexTriangleCallback.cpp', 'Bullet/CollisionShapes/EmptyShape.cpp', 'Bullet/CollisionShapes/OptimizedBvh.cpp', 'Bullet/CollisionShapes/TriangleCallback.cpp', 'Bullet/CollisionShapes/TriangleIndexVertexArray.cpp', 'Bullet/NarrowPhaseCollision/ManifoldContactAddResult.cpp'. Sorry, no armatures fix yet.
Diffstat (limited to 'extern')
-rw-r--r--extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h1
-rw-r--r--extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp30
-rw-r--r--extern/bullet/Bullet/CollisionShapes/BoxShape.h2
-rw-r--r--extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp134
-rw-r--r--extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.h53
-rw-r--r--extern/bullet/Bullet/CollisionShapes/CollisionMargin.h (renamed from extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h)0
-rw-r--r--extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp24
-rw-r--r--extern/bullet/Bullet/CollisionShapes/ConvexHullShape.h1
-rw-r--r--extern/bullet/Bullet/CollisionShapes/ConvexShape.h2
-rw-r--r--extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp86
-rw-r--r--extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h49
-rw-r--r--extern/bullet/Bullet/CollisionShapes/EmptyShape.cpp45
-rw-r--r--extern/bullet/Bullet/CollisionShapes/EmptyShape.h75
-rw-r--r--extern/bullet/Bullet/CollisionShapes/MultiSphereShape.cpp2
-rw-r--r--extern/bullet/Bullet/CollisionShapes/OptimizedBvh.cpp268
-rw-r--r--extern/bullet/Bullet/CollisionShapes/OptimizedBvh.h92
-rw-r--r--extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.cpp25
-rw-r--r--extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.h3
-rw-r--r--extern/bullet/Bullet/CollisionShapes/Simplex1to4Shape.h4
-rw-r--r--extern/bullet/Bullet/CollisionShapes/SphereShape.cpp2
-rw-r--r--extern/bullet/Bullet/CollisionShapes/StridingMeshInterface.cpp63
-rw-r--r--extern/bullet/Bullet/CollisionShapes/StridingMeshInterface.h19
-rw-r--r--extern/bullet/Bullet/CollisionShapes/TriangleCallback.cpp23
-rw-r--r--extern/bullet/Bullet/CollisionShapes/TriangleCallback.h10
-rw-r--r--extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp49
-rw-r--r--extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.h46
-rw-r--r--extern/bullet/Bullet/CollisionShapes/TriangleMesh.cpp19
-rw-r--r--extern/bullet/Bullet/CollisionShapes/TriangleMesh.h6
-rw-r--r--extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp101
-rw-r--r--extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h8
-rw-r--r--extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.h3
-rw-r--r--extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h2
-rw-r--r--extern/bullet/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.cpp33
-rw-r--r--extern/bullet/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.h32
-rw-r--r--extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp2
-rw-r--r--extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp25
-rw-r--r--extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.h2
-rw-r--r--extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp35
-rw-r--r--extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h66
-rw-r--r--extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp2
-rw-r--r--extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp5
-rw-r--r--extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp15
-rw-r--r--extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h3
-rw-r--r--extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp3
-rw-r--r--extern/bullet/LinearMath/SimdQuadWord.h32
-rw-r--r--extern/bullet/LinearMath/SimdScalar.h15
-rw-r--r--extern/bullet/SConscript11
47 files changed, 1355 insertions, 173 deletions
diff --git a/extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h b/extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h
index 2f4ed2c1653..235e2f1e465 100644
--- a/extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h
+++ b/extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h
@@ -36,6 +36,7 @@ IMPLICIT_CONVEX_SHAPES_START_HERE,
CONCAVE_SHAPES_START_HERE,
//keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
TRIANGLE_MESH_SHAPE_PROXYTYPE,
+ EMPTY_SHAPE_PROXYTYPE,
MAX_BROADPHASE_COLLISION_TYPES
};
diff --git a/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp b/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp
index 663cae2949a..823bfdc9edd 100644
--- a/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp
+++ b/extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp
@@ -16,7 +16,7 @@
#include "SimdTransform.h"
#include "SimdMatrix3x3.h"
#include <vector>
-#include "NarrowPhaseCollision/CollisionMargin.h"
+#include "CollisionShapes/CollisionMargin.h"
SimpleBroadphase::SimpleBroadphase()
: m_firstFreeProxy(0),
@@ -63,12 +63,34 @@ BroadphaseProxy* SimpleBroadphase::CreateProxy( void *object, int type, const Si
}
void SimpleBroadphase::DestroyProxy(BroadphaseProxy* proxy)
{
- m_numProxies--;
+
+ int i;
+
BroadphaseProxy* proxy1 = &m_proxies[0];
int index = proxy - proxy1;
m_freeProxies[--m_firstFreeProxy] = index;
+ for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--)
+ {
+ BroadphasePair& pair = m_OverlappingPairs[i];
+ if (pair.m_pProxy0 == proxy ||
+ pair.m_pProxy1 == proxy)
+ {
+ RemoveOverlappingPair(pair);
+ }
+ }
+
+ for (i=0;i<m_numProxies;i++)
+ {
+ if (m_pProxies[i] == proxy)
+ {
+ m_proxies[i] = m_proxies[m_numProxies-1];
+ break;
+ }
+ }
+ m_numProxies--;
+
}
SimpleBroadphaseProxy* SimpleBroadphase::GetSimpleProxyFromProxy(BroadphaseProxy* proxy)
@@ -76,7 +98,7 @@ SimpleBroadphaseProxy* SimpleBroadphase::GetSimpleProxyFromProxy(BroadphaseProxy
SimpleBroadphaseProxy* proxy0 = static_cast<SimpleBroadphaseProxy*>(proxy);
int index = proxy0 - &m_proxies[0];
- assert(index < m_numProxies);
+ //assert(index < m_numProxies);
SimpleBroadphaseProxy* sbp = &m_proxies[index];
return sbp;
@@ -118,6 +140,8 @@ void SimpleBroadphase::CleanProxyFromPairs(BroadphaseProxy* proxy)
void SimpleBroadphase::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
{
+
+
BroadphasePair pair(*proxy0,*proxy1);
m_OverlappingPairs[m_NumOverlapBroadphasePair] = pair;
diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.h b/extern/bullet/Bullet/CollisionShapes/BoxShape.h
index 1cb0d822b76..bbc07f90f5b 100644
--- a/extern/bullet/Bullet/CollisionShapes/BoxShape.h
+++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.h
@@ -13,7 +13,7 @@
#define OBB_BOX_MINKOWSKI_H
#include "PolyhedralConvexShape.h"
-#include "NarrowPhaseCollision/CollisionMargin.h"
+#include "CollisionShapes/CollisionMargin.h"
#include "BroadphaseCollision/BroadphaseProxy.h"
#include "SimdPoint3.h"
#include "SimdMinMax.h"
diff --git a/extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp b/extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp
new file mode 100644
index 00000000000..0c94b884597
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2006 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+
+//#define DISABLE_BVH
+
+
+#include "CollisionShapes/BvhTriangleMeshShape.h"
+#include "CollisionShapes/OptimizedBvh.h"
+
+///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
+///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
+BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface* meshInterface)
+:TriangleMeshShape(meshInterface)
+{
+ //construct bvh from meshInterface
+#ifndef DISABLE_BVH
+
+ m_bvh = new OptimizedBvh();
+ m_bvh->Build(meshInterface);
+
+#endif //DISABLE_BVH
+
+}
+
+BvhTriangleMeshShape::~BvhTriangleMeshShape()
+{
+ delete m_bvh;
+}
+
+//perform bvh tree traversal and report overlapping triangles to 'callback'
+void BvhTriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+
+#ifdef DISABLE_BVH
+ //brute force traverse all triangles
+ TriangleMeshShape::ProcessAllTriangles(callback,aabbMin,aabbMax);
+#else
+
+ //first get all the nodes
+
+
+ struct MyNodeOverlapCallback : public NodeOverlapCallback
+ {
+ StridingMeshInterface* m_meshInterface;
+ TriangleCallback* m_callback;
+ SimdVector3 m_triangle[3];
+
+
+ MyNodeOverlapCallback(TriangleCallback* callback,StridingMeshInterface* meshInterface)
+ :m_callback(callback),
+ m_meshInterface(meshInterface)
+ {
+ }
+
+ virtual void ProcessNode(const OptimizedBvhNode* node)
+ {
+ const unsigned char *vertexbase;
+ int numverts;
+ PHY_ScalarType type;
+ int stride;
+ const unsigned char *indexbase;
+ int indexstride;
+ int numfaces;
+ PHY_ScalarType indicestype;
+
+
+ m_meshInterface->getLockedReadOnlyVertexIndexBase(
+ &vertexbase,
+ numverts,
+ type,
+ stride,
+ &indexbase,
+ indexstride,
+ numfaces,
+ indicestype,
+ node->m_subPart);
+
+ int* gfxbase = (int*)(indexbase+node->m_triangleIndex*indexstride);
+
+ const SimdVector3& meshScaling = m_meshInterface->getScaling();
+ for (int j=2;j>=0;j--)
+ {
+
+ int graphicsindex = gfxbase[j];
+#ifdef DEBUG_TRIANGLE_MESH
+ printf("%d ,",graphicsindex);
+#endif //DEBUG_TRIANGLE_MESH
+ float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+
+ m_triangle[j] = SimdVector3(
+ graphicsbase[0]*meshScaling.getX(),
+ graphicsbase[1]*meshScaling.getY(),
+ graphicsbase[2]*meshScaling.getZ());
+#ifdef DEBUG_TRIANGLE_MESH
+ printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z());
+#endif //DEBUG_TRIANGLE_MESH
+ }
+
+ m_callback->ProcessTriangle(m_triangle);
+ m_meshInterface->unLockReadOnlyVertexBase(node->m_subPart);
+ }
+
+ };
+
+ MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface);
+
+ m_bvh->ReportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
+
+
+#endif//DISABLE_BVH
+
+
+}
+
+
+void BvhTriangleMeshShape::setLocalScaling(const SimdVector3& scaling)
+{
+ if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON)
+ {
+ TriangleMeshShape::setLocalScaling(scaling);
+ delete m_bvh;
+ m_bvh = new OptimizedBvh();
+ m_bvh->Build(m_meshInterface);
+ //rebuild the bvh...
+ }
+}
diff --git a/extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.h b/extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.h
new file mode 100644
index 00000000000..86cd1e56485
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/BvhTriangleMeshShape.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2006 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+#ifndef BVH_TRIANGLE_MESH_SHAPE_H
+#define BVH_TRIANGLE_MESH_SHAPE_H
+
+#include "CollisionShapes/TriangleMeshShape.h"
+#include "CollisionShapes/OptimizedBvh.h"
+
+///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
+///Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
+class BvhTriangleMeshShape : public TriangleMeshShape
+{
+
+ OptimizedBvh* m_bvh;
+
+
+public:
+ BvhTriangleMeshShape(StridingMeshInterface* meshInterface);
+
+ virtual ~BvhTriangleMeshShape();
+
+
+ /*
+ virtual int GetShapeType() const
+ {
+ return TRIANGLE_MESH_SHAPE_PROXYTYPE;
+ }
+ */
+
+
+
+ virtual void ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+
+ //debugging
+ virtual char* GetName()const {return "BVHTRIANGLEMESH";}
+
+
+ virtual void setLocalScaling(const SimdVector3& scaling);
+
+
+
+};
+
+#endif //BVH_TRIANGLE_MESH_SHAPE_H
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h b/extern/bullet/Bullet/CollisionShapes/CollisionMargin.h
index 41009dcd110..41009dcd110 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h
+++ b/extern/bullet/Bullet/CollisionShapes/CollisionMargin.h
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp
index 1cea1d02f08..fdb7a4d801a 100644
--- a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp
@@ -10,7 +10,7 @@
*/
#include "ConvexHullShape.h"
-#include "NarrowPhaseCollision/CollisionMargin.h"
+#include "CollisionShapes/CollisionMargin.h"
#include "SimdQuaternion.h"
@@ -75,29 +75,7 @@ SimdVector3 ConvexHullShape::LocalGetSupportingVertex(const SimdVector3& vec)con
-void ConvexHullShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
-{
- //not yet, return box inertia
-
- float margin = GetMargin();
-
- SimdTransform ident;
- ident.setIdentity();
- SimdVector3 aabbMin,aabbMax;
- GetAabb(ident,aabbMin,aabbMax);
- SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;
- SimdScalar lx=2.f*(halfExtents.x()+margin);
- SimdScalar ly=2.f*(halfExtents.y()+margin);
- SimdScalar lz=2.f*(halfExtents.z()+margin);
- const SimdScalar x2 = lx*lx;
- const SimdScalar y2 = ly*ly;
- const SimdScalar z2 = lz*lz;
- const SimdScalar scaledmass = mass * 0.08333333f;
-
- inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));
-
-}
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.h b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.h
index a06bcb1e9dc..76c198ab93a 100644
--- a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.h
+++ b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.h
@@ -35,7 +35,6 @@ public:
virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec)const;
virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
- virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
virtual int GetShapeType()const { return CONVEX_HULL_SHAPE_PROXYTYPE; }
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexShape.h b/extern/bullet/Bullet/CollisionShapes/ConvexShape.h
index b1e6f9e9924..82986200497 100644
--- a/extern/bullet/Bullet/CollisionShapes/ConvexShape.h
+++ b/extern/bullet/Bullet/CollisionShapes/ConvexShape.h
@@ -18,7 +18,7 @@
#include "SimdTransform.h"
#include "SimdMatrix3x3.h"
#include <vector>
-#include "NarrowPhaseCollision/CollisionMargin.h"
+#include "CollisionShapes/CollisionMargin.h"
//todo: get rid of this ConvexCastResult thing!
struct ConvexCastResult;
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp
new file mode 100644
index 00000000000..dbaaaf243fa
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp
@@ -0,0 +1,86 @@
+#include "ConvexTriangleCallback.h"
+#include "NarrowPhaseCollision/PersistentManifold.h"
+#include "NarrowPhaseCollision/ManifoldContactAddResult.h"
+#include "NarrowPhaseCollision/GjkPairDetector.h"
+#include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h"
+
+
+
+#include "TriangleShape.h"
+
+//m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
+ //m_dispatcher->ReleaseManifold( m_manifoldPtr );
+
+ConvexTriangleCallback::ConvexTriangleCallback(PersistentManifold* manifold,ConvexShape* convexShape,const SimdTransform&convexTransform,const SimdTransform& triangleMeshTransform)
+:m_triangleMeshTransform(triangleMeshTransform),
+ m_convexTransform(convexTransform),
+ m_convexShape(convexShape),
+ m_manifoldPtr(manifold),
+ m_triangleCount(0)
+{
+}
+
+ConvexTriangleCallback::~ConvexTriangleCallback()
+{
+
+}
+
+
+void ConvexTriangleCallback::ClearCache()
+{
+ m_manifoldPtr->ClearManifold();
+};
+
+
+
+void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle)
+{
+
+ //triangle, convex
+ TriangleShape tm(triangle[0],triangle[1],triangle[2]);
+ tm.SetMargin(m_collisionMarginTriangle);
+ GjkPairDetector::ClosestPointInput input;
+ input.m_transformA = m_triangleMeshTransform;
+ input.m_transformB = m_convexTransform;
+
+ ManifoldContactAddResult output(m_triangleMeshTransform,m_convexTransform,m_manifoldPtr);
+
+
+ VoronoiSimplexSolver simplexSolver;
+ MinkowskiPenetrationDepthSolver penetrationDepthSolver;
+
+ GjkPairDetector gjkDetector(&tm,m_convexShape,&simplexSolver,&penetrationDepthSolver);
+
+ gjkDetector.SetMinkowskiA(&tm);
+ gjkDetector.SetMinkowskiB(m_convexShape);
+ input.m_maximumDistanceSquared = tm.GetMargin()+ m_convexShape->GetMargin() + m_manifoldPtr->GetManifoldMargin();
+ input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+
+ input.m_maximumDistanceSquared = 1e30f;//?
+
+
+ gjkDetector.GetClosestPoints(input,output);
+
+
+}
+
+
+
+void ConvexTriangleCallback::Update(float collisionMarginTriangle)
+{
+ m_triangleCount = 0;
+ m_collisionMarginTriangle = collisionMarginTriangle;
+
+ SimdTransform boxInTriangleSpace;
+ boxInTriangleSpace = m_triangleMeshTransform.inverse() * m_convexTransform;
+
+ m_convexShape->GetAabb(boxInTriangleSpace,m_aabbMin,m_aabbMax);
+
+ float extraMargin = CONVEX_DISTANCE_MARGIN;
+
+ SimdVector3 extra(extraMargin,extraMargin,extraMargin);
+
+ m_aabbMax += extra;
+ m_aabbMin -= extra;
+
+}
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h
new file mode 100644
index 00000000000..8e0e446a549
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h
@@ -0,0 +1,49 @@
+#ifndef CONVEX_TRIANGLE_CALLBACK_H
+#define CONVEX_TRIANGLE_CALLBACK_H
+
+#include "TriangleCallback.h"
+class ConvexShape;
+class PersistentManifold;
+#include "SimdTransform.h"
+///ConvexTriangleCallback processes the narrowphase convex-triangle collision detection
+class ConvexTriangleCallback: public TriangleCallback
+{
+ SimdVector3 m_aabbMin;
+ SimdVector3 m_aabbMax ;
+
+ SimdTransform m_triangleMeshTransform;
+ SimdTransform m_convexTransform;
+
+// bool m_useContinuous;
+ float m_collisionMarginTriangle;
+
+public:
+int m_triangleCount;
+
+ ConvexShape* m_convexShape;
+
+ PersistentManifold* m_manifoldPtr;
+
+ ConvexTriangleCallback(PersistentManifold* manifold,ConvexShape* convexShape,const SimdTransform&convexTransform,const SimdTransform& triangleMeshTransform);
+
+ void Update(float collisionMarginTriangle);
+
+ virtual ~ConvexTriangleCallback();
+
+ virtual void ProcessTriangle(SimdVector3* triangle);
+
+ void ClearCache();
+
+ inline const SimdVector3& GetAabbMin() const
+ {
+ return m_aabbMin;
+ }
+ inline const SimdVector3& GetAabbMax() const
+ {
+ return m_aabbMax;
+ }
+
+};
+
+
+#endif //CONVEX_TRIANGLE_CALLBACK_H \ No newline at end of file
diff --git a/extern/bullet/Bullet/CollisionShapes/EmptyShape.cpp b/extern/bullet/Bullet/CollisionShapes/EmptyShape.cpp
new file mode 100644
index 00000000000..04954dd750e
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/EmptyShape.cpp
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+ */
+
+#include "EmptyShape.h"
+
+
+#include "CollisionShape.h"
+
+
+EmptyShape::EmptyShape()
+{
+}
+
+
+EmptyShape::~EmptyShape()
+{
+}
+
+
+ ///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+void EmptyShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
+{
+ SimdVector3 margin(GetMargin(),GetMargin(),GetMargin());
+
+ aabbMin = t.getOrigin() - margin;
+
+ aabbMax = t.getOrigin() + margin;
+
+}
+
+void EmptyShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+ assert(0);
+}
+
+
+
diff --git a/extern/bullet/Bullet/CollisionShapes/EmptyShape.h b/extern/bullet/Bullet/CollisionShapes/EmptyShape.h
new file mode 100644
index 00000000000..7ecbb05f4cc
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/EmptyShape.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+ */
+
+#ifndef EMPTY_SHAPE_H
+#define EMPTY_SHAPE_H
+
+#include "CollisionShape.h"
+
+#include "SimdVector3.h"
+#include "SimdTransform.h"
+#include "SimdMatrix3x3.h"
+#include <vector>
+#include "CollisionShapes/CollisionMargin.h"
+
+
+
+
+/// EmptyShape is a collision shape without actual collision detection. It can be replaced by another shape during runtime
+class EmptyShape : public CollisionShape
+{
+public:
+ EmptyShape();
+
+ virtual ~EmptyShape();
+
+
+ ///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+ void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const;
+
+
+ virtual void setLocalScaling(const SimdVector3& scaling)
+ {
+ m_localScaling = scaling;
+ }
+ virtual const SimdVector3& getLocalScaling() const
+ {
+ return m_localScaling;
+ }
+
+ virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+
+ virtual int GetShapeType() const { return EMPTY_SHAPE_PROXYTYPE;}
+
+ virtual void SetMargin(float margin)
+ {
+ m_collisionMargin = margin;
+ }
+ virtual float GetMargin() const
+ {
+ return m_collisionMargin;
+ }
+ virtual char* GetName()const
+ {
+ return "Empty";
+ }
+
+
+private:
+ SimdScalar m_collisionMargin;
+protected:
+ SimdVector3 m_localScaling;
+
+};
+
+
+
+#endif //EMPTY_SHAPE_H
diff --git a/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.cpp b/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.cpp
index 93787081a34..d1d5aa92897 100644
--- a/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/MultiSphereShape.cpp
@@ -9,7 +9,7 @@
* It is provided "as is" without express or implied warranty.
*/
#include "MultiSphereShape.h"
-#include "NarrowPhaseCollision/CollisionMargin.h"
+#include "CollisionShapes/CollisionMargin.h"
#include "SimdQuaternion.h"
MultiSphereShape::MultiSphereShape (const SimdVector3& inertiaHalfExtents,const SimdVector3* positions,const SimdScalar* radi,int numSpheres)
diff --git a/extern/bullet/Bullet/CollisionShapes/OptimizedBvh.cpp b/extern/bullet/Bullet/CollisionShapes/OptimizedBvh.cpp
new file mode 100644
index 00000000000..fe3a51883ec
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/OptimizedBvh.cpp
@@ -0,0 +1,268 @@
+/*
+ * Copyright (c) 2006 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+
+#include "OptimizedBvh.h"
+#include "StridingMeshInterface.h"
+#include "AabbUtil2.h"
+
+
+
+void OptimizedBvh::Build(StridingMeshInterface* triangles)
+{
+ int countTriangles = 0;
+
+
+
+ // NodeArray triangleNodes;
+
+ struct NodeTriangleCallback : public InternalTriangleIndexCallback
+ {
+ NodeArray& m_triangleNodes;
+
+ NodeTriangleCallback(NodeArray& triangleNodes)
+ :m_triangleNodes(triangleNodes)
+ {
+
+ }
+
+ virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex)
+ {
+
+ OptimizedBvhNode node;
+ node.m_aabbMin = SimdVector3(1e30f,1e30f,1e30f);
+ node.m_aabbMax = SimdVector3(-1e30f,-1e30f,-1e30f);
+ node.m_aabbMin.setMin(triangle[0]);
+ node.m_aabbMax.setMax(triangle[0]);
+ node.m_aabbMin.setMin(triangle[1]);
+ node.m_aabbMax.setMax(triangle[1]);
+ node.m_aabbMin.setMin(triangle[2]);
+ node.m_aabbMax.setMax(triangle[2]);
+
+ node.m_escapeIndex = -1;
+ node.m_leftChild = 0;
+ node.m_rightChild = 0;
+
+
+ //for child nodes
+ node.m_subPart = partId;
+ node.m_triangleIndex = triangleIndex;
+
+
+ m_triangleNodes.push_back(node);
+ }
+ };
+
+
+
+ NodeTriangleCallback callback(m_leafNodes);
+
+ SimdVector3 aabbMin(-1e30f,-1e30f,-1e30f);
+ SimdVector3 aabbMax(1e30f,1e30f,1e30f);
+
+ triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
+
+ //now we have an array of leafnodes in m_leafNodes
+
+ m_contiguousNodes = new OptimizedBvhNode[2*m_leafNodes.size()];
+ m_curNodeIndex = 0;
+
+ m_rootNode1 = BuildTree(m_leafNodes,0,m_leafNodes.size());
+
+
+ ///create the leafnodes first
+// OptimizedBvhNode* leafNodes = new OptimizedBvhNode;
+}
+
+
+OptimizedBvhNode* OptimizedBvh::BuildTree (NodeArray& leafNodes,int startIndex,int endIndex)
+{
+
+ int numIndices =endIndex-startIndex;
+ assert(numIndices>0);
+
+ int curIndex = m_curNodeIndex;
+
+ if (numIndices==1)
+ {
+ return new (&m_contiguousNodes[m_curNodeIndex++]) OptimizedBvhNode(leafNodes[startIndex]);
+ }
+ //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+ int splitAxis = CalcSplittingAxis(leafNodes,startIndex,endIndex);
+
+ int splitIndex = SortAndCalcSplittingIndex(leafNodes,startIndex,endIndex,splitAxis);
+
+ OptimizedBvhNode* internalNode = &m_contiguousNodes[m_curNodeIndex++];
+
+ internalNode->m_aabbMax.setValue(-1e30f,-1e30f,-1e30f);
+ internalNode->m_aabbMin.setValue(1e30f,1e30f,1e30f);
+
+ for (int i=startIndex;i<endIndex;i++)
+ {
+ internalNode->m_aabbMax.setMax(leafNodes[i].m_aabbMax);
+ internalNode->m_aabbMin.setMin(leafNodes[i].m_aabbMin);
+ }
+
+
+
+ //internalNode->m_escapeIndex;
+ internalNode->m_leftChild = BuildTree(leafNodes,startIndex,splitIndex);
+ internalNode->m_rightChild = BuildTree(leafNodes,splitIndex,endIndex);
+
+ internalNode->m_escapeIndex = m_curNodeIndex - curIndex;
+ return internalNode;
+}
+
+int OptimizedBvh::SortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis)
+{
+ int splitIndex =startIndex;
+ int numIndices = endIndex - startIndex;
+
+ SimdVector3 means(0.f,0.f,0.f);
+ for (int i=startIndex;i<endIndex;i++)
+ {
+ SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+ means+=center;
+ }
+ means *= (1.f/(float)numIndices);
+
+ float splitValue = means[splitAxis];
+
+ //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+ for (int i=startIndex;i<endIndex;i++)
+ {
+ SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+ if (center[splitAxis] > splitValue)
+ {
+ //swap
+ OptimizedBvhNode tmp = leafNodes[i];
+ leafNodes[i] = leafNodes[splitIndex];
+ leafNodes[splitIndex] = tmp;
+ splitIndex++;
+ }
+ }
+ if ((splitIndex==startIndex) || (splitIndex == (endIndex-1)))
+ {
+ splitIndex = startIndex+ (numIndices>>1);
+ }
+ return splitIndex;
+}
+
+
+int OptimizedBvh::CalcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex)
+{
+ SimdVector3 means(0.f,0.f,0.f);
+ int numIndices = endIndex-startIndex;
+
+ for (int i=startIndex;i<endIndex;i++)
+ {
+ SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+ means+=center;
+ }
+ means *= (1.f/(float)numIndices);
+
+ SimdVector3 variance(0.f,0.f,0.f);
+
+ for (int i=startIndex;i<endIndex;i++)
+ {
+ SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
+ SimdVector3 diff2 = center-means;
+ diff2 = diff2 * diff2;
+ variance += diff2;
+ }
+ variance *= (1.f/ ((float)numIndices-1) );
+
+ int biggestAxis = variance.maxAxis();
+ return biggestAxis;
+
+}
+
+
+
+void OptimizedBvh::ReportAabbOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+ if (aabbMin.length() > 1000.f)
+ {
+ for (int i=0;i<m_leafNodes.size();i++)
+ {
+ const OptimizedBvhNode& node = m_leafNodes[i];
+ nodeCallback->ProcessNode(&node);
+ }
+ } else
+ {
+ //WalkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
+ WalkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
+ }
+}
+
+void OptimizedBvh::WalkTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+ bool aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
+ if (aabbOverlap)
+ {
+ bool isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
+ if (isLeafNode)
+ {
+ nodeCallback->ProcessNode(rootNode);
+ } else
+ {
+ WalkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax);
+ WalkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax);
+ }
+ }
+
+}
+
+int maxIterations = 0;
+
+void OptimizedBvh::WalkStacklessTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+ int curIndex = 0;
+ int walkIterations = 0;
+
+ while (curIndex < m_curNodeIndex)
+ {
+ //catch bugs in tree data
+ assert (walkIterations < m_curNodeIndex);
+
+ walkIterations++;
+ bool aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
+ bool isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
+
+ if (isLeafNode && aabbOverlap)
+ {
+ nodeCallback->ProcessNode(rootNode);
+ }
+
+ if (aabbOverlap || isLeafNode)
+ {
+ rootNode++;
+ curIndex++;
+ } else
+ {
+ int escapeIndex = rootNode->m_escapeIndex;
+ rootNode += escapeIndex;
+ curIndex += escapeIndex;
+ }
+
+ }
+
+ if (maxIterations < walkIterations)
+ maxIterations = walkIterations;
+
+}
+
+
+void OptimizedBvh::ReportSphereOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+
+}
+
diff --git a/extern/bullet/Bullet/CollisionShapes/OptimizedBvh.h b/extern/bullet/Bullet/CollisionShapes/OptimizedBvh.h
new file mode 100644
index 00000000000..911bb9af432
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/OptimizedBvh.h
@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2006 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+
+#ifndef OPTIMIZED_BVH_H
+#define OPTIMIZED_BVH_H
+#include "SimdVector3.h"
+#include <vector>
+
+class StridingMeshInterface;
+
+/// OptimizedBvhNode contains both internal and leaf node information.
+/// It hasn't been optimized yet for storage. Some obvious optimizations are:
+/// Removal of the pointers (can already be done, they are not used for traversal)
+/// 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 OptimizedBvhNode
+{
+
+ SimdVector3 m_aabbMin;
+ SimdVector3 m_aabbMax;
+
+//these 2 pointers are obsolete, the stackless traversal just uses the escape index
+ OptimizedBvhNode* m_leftChild;
+ OptimizedBvhNode* m_rightChild;
+
+ int m_escapeIndex;
+
+ //for child nodes
+ int m_subPart;
+ int m_triangleIndex;
+
+};
+
+class NodeOverlapCallback
+{
+public:
+ virtual void ProcessNode(const OptimizedBvhNode* node) = 0;
+};
+
+typedef std::vector<OptimizedBvhNode> NodeArray;
+
+
+///OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future)
+class OptimizedBvh
+{
+ OptimizedBvhNode* m_rootNode1;
+
+ OptimizedBvhNode* m_contiguousNodes;
+ int m_curNodeIndex;
+
+ int m_numNodes;
+
+ NodeArray m_leafNodes;
+
+public:
+ OptimizedBvh() :m_rootNode1(0), m_numNodes(0) { }
+
+ void Build(StridingMeshInterface* triangles);
+
+ OptimizedBvhNode* BuildTree (NodeArray& leafNodes,int startIndex,int endIndex);
+
+ int CalcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex);
+
+ int SortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis);
+
+ void WalkTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+ void WalkStacklessTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+
+ //OptimizedBvhNode* GetRootNode() { return m_rootNode1;}
+
+ int GetNumNodes() { return m_numNodes;}
+
+ void ReportAabbOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+ void ReportSphereOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+
+};
+
+
+#endif //OPTIMIZED_BVH_H \ No newline at end of file
diff --git a/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.cpp b/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.cpp
index 55f29bab960..e480b8911c7 100644
--- a/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.cpp
@@ -50,3 +50,28 @@ SimdVector3 PolyhedralConvexShape::LocalGetSupportingVertexWithoutMargin(const S
return supVec;
}
+
+void PolyhedralConvexShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
+{
+ //not yet, return box inertia
+
+ float margin = GetMargin();
+
+ SimdTransform ident;
+ ident.setIdentity();
+ SimdVector3 aabbMin,aabbMax;
+ GetAabb(ident,aabbMin,aabbMax);
+ SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;
+
+ SimdScalar lx=2.f*(halfExtents.x()+margin);
+ SimdScalar ly=2.f*(halfExtents.y()+margin);
+ SimdScalar lz=2.f*(halfExtents.z()+margin);
+ const SimdScalar x2 = lx*lx;
+ const SimdScalar y2 = ly*ly;
+ const SimdScalar z2 = lz*lz;
+ const SimdScalar scaledmass = mass * 0.08333333f;
+
+ inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));
+
+}
+
diff --git a/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.h b/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.h
index 9c3764ae014..f1462e21f97 100644
--- a/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.h
+++ b/extern/bullet/Bullet/CollisionShapes/PolyhedralConvexShape.h
@@ -27,6 +27,9 @@ public:
//brute force implementations
virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
+ virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
+
+
virtual int GetNumVertices() const = 0 ;
virtual int GetNumEdges() const = 0;
diff --git a/extern/bullet/Bullet/CollisionShapes/Simplex1to4Shape.h b/extern/bullet/Bullet/CollisionShapes/Simplex1to4Shape.h
index a7533efa5cf..7c61cb661d7 100644
--- a/extern/bullet/Bullet/CollisionShapes/Simplex1to4Shape.h
+++ b/extern/bullet/Bullet/CollisionShapes/Simplex1to4Shape.h
@@ -38,10 +38,6 @@ public:
m_numVertices = 0;
}
- virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
- {
- inertia = SimdVector3(1.f,1.f,1.f);
- }
virtual int GetShapeType() const{ return TETRAHEDRAL_SHAPE_PROXYTYPE; }
diff --git a/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp b/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp
index 8c28cca172e..bbffb8137de 100644
--- a/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp
@@ -10,7 +10,7 @@
*/
#include "SphereShape.h"
-#include "NarrowPhaseCollision/CollisionMargin.h"
+#include "CollisionShapes/CollisionMargin.h"
#include "SimdQuaternion.h"
diff --git a/extern/bullet/Bullet/CollisionShapes/StridingMeshInterface.cpp b/extern/bullet/Bullet/CollisionShapes/StridingMeshInterface.cpp
index 8fd761f06cb..cbbbdcbe99e 100644
--- a/extern/bullet/Bullet/CollisionShapes/StridingMeshInterface.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/StridingMeshInterface.cpp
@@ -14,3 +14,66 @@ StridingMeshInterface::~StridingMeshInterface()
{
}
+
+
+void StridingMeshInterface::InternalProcessAllTriangles(InternalTriangleIndexCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
+{
+
+ SimdVector3 meshScaling = getScaling();
+
+ int numtotalphysicsverts = 0;
+ int part,graphicssubparts = getNumSubParts();
+ for (part=0;part<graphicssubparts ;part++)
+ {
+ const unsigned char * vertexbase;
+ const unsigned char * indexbase;
+ int indexstride;
+ PHY_ScalarType type;
+ PHY_ScalarType gfxindextype;
+ int stride,numverts,numtriangles;
+ getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
+
+ numtotalphysicsverts+=numtriangles*3; //upper bound
+
+
+ int gfxindex;
+ SimdVector3 triangle[3];
+
+ for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+ {
+
+ int graphicsindex=0;
+
+#ifdef DEBUG_TRIANGLE_MESH
+ printf("triangle indices:\n");
+#endif //DEBUG_TRIANGLE_MESH
+ ASSERT(gfxindextype == PHY_INTEGER);
+ int* gfxbase = (int*)(indexbase+gfxindex*indexstride);
+
+ for (int j=2;j>=0;j--)
+ {
+
+ graphicsindex = gfxbase[j];
+#ifdef DEBUG_TRIANGLE_MESH
+ printf("%d ,",graphicsindex);
+#endif //DEBUG_TRIANGLE_MESH
+ float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+
+ triangle[j] = SimdVector3(
+ graphicsbase[0]*meshScaling.getX(),
+ graphicsbase[1]*meshScaling.getY(),
+ graphicsbase[2]*meshScaling.getZ());
+#ifdef DEBUG_TRIANGLE_MESH
+ printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z());
+#endif //DEBUG_TRIANGLE_MESH
+ }
+
+
+ //check aabb in triangle-space, before doing this
+ callback->InternalProcessTriangleIndex(triangle,part,gfxindex);
+
+ }
+
+ unLockReadOnlyVertexBase(part);
+ }
+} \ No newline at end of file
diff --git a/extern/bullet/Bullet/CollisionShapes/StridingMeshInterface.h b/extern/bullet/Bullet/CollisionShapes/StridingMeshInterface.h
index ba8a925c8dd..82f89b4dbe0 100644
--- a/extern/bullet/Bullet/CollisionShapes/StridingMeshInterface.h
+++ b/extern/bullet/Bullet/CollisionShapes/StridingMeshInterface.h
@@ -12,6 +12,7 @@
#define STRIDING_MESHINTERFACE_H
#include "SimdVector3.h"
+#include "TriangleCallback.h"
/// PHY_ScalarType enumerates possible scalar types.
/// See the StridingMeshInterface for its use
@@ -38,26 +39,36 @@ class StridingMeshInterface
}
virtual ~StridingMeshInterface();
+
+
+
+ void InternalProcessAllTriangles(InternalTriangleIndexCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+
+
/// get read and write access to a subpart of a triangle mesh
/// this subpart has a continuous array of vertices and indices
/// in this way the mesh can be handled as chunks of memory with striding
/// very similar to OpenGL vertexarray support
/// make a call to unLockVertexBase when the read and write access is finished
- virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0)=0;
+ virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0)=0;
-
+ virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const=0;
+
/// unLockVertexBase finishes the access to a subpart of the triangle mesh
/// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
virtual void unLockVertexBase(int subpart)=0;
+ virtual void unLockReadOnlyVertexBase(int subpart) const=0;
+
+
/// getNumSubParts returns the number of seperate subparts
/// each subpart has a continuous array of vertices and indices
- virtual int getNumSubParts()=0;
+ virtual int getNumSubParts() const=0;
virtual void preallocateVertices(int numverts)=0;
virtual void preallocateIndices(int numindices)=0;
- const SimdVector3& getScaling() {
+ const SimdVector3& getScaling() const {
return m_scaling;
}
void setScaling(const SimdVector3& scaling)
diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleCallback.cpp b/extern/bullet/Bullet/CollisionShapes/TriangleCallback.cpp
new file mode 100644
index 00000000000..6fc69355c2d
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/TriangleCallback.cpp
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2006 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+
+#include "TriangleCallback.h"
+
+TriangleCallback::~TriangleCallback()
+{
+
+}
+
+
+InternalTriangleIndexCallback::~InternalTriangleIndexCallback()
+{
+
+} \ No newline at end of file
diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleCallback.h b/extern/bullet/Bullet/CollisionShapes/TriangleCallback.h
index 0e464d09e8b..cbffeeb9839 100644
--- a/extern/bullet/Bullet/CollisionShapes/TriangleCallback.h
+++ b/extern/bullet/Bullet/CollisionShapes/TriangleCallback.h
@@ -22,4 +22,14 @@ public:
virtual void ProcessTriangle(SimdVector3* triangle) = 0;
};
+class InternalTriangleIndexCallback
+{
+public:
+
+ virtual ~InternalTriangleIndexCallback();
+ virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex) = 0;
+};
+
+
+
#endif //TRIANGLE_CALLBACK_H
diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp b/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp
new file mode 100644
index 00000000000..711a9128048
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2006 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+
+#include "TriangleIndexVertexArray.h"
+
+TriangleIndexVertexArray::TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride)
+:m_numTriangleIndices(numTriangleIndices),
+m_triangleIndexBase(triangleIndexBase),
+m_triangleIndexStride(triangleIndexStride),
+m_numVertices(numVertices),
+m_vertexBase(vertexBase),
+m_vertexStride(vertexStride)
+{
+}
+
+void TriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart)
+{
+ numverts = m_numVertices;
+ (*vertexbase) = (unsigned char *)m_vertexBase;
+ type = PHY_FLOAT;
+ vertexStride = m_vertexStride;
+
+ numfaces = m_numTriangleIndices;
+ (*indexbase) = (unsigned char *)m_triangleIndexBase;
+ indexstride = m_triangleIndexStride;
+ indicestype = PHY_INTEGER;
+}
+
+void TriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const
+{
+ numverts = m_numVertices;
+ (*vertexbase) = (unsigned char *)m_vertexBase;
+ type = PHY_FLOAT;
+ vertexStride = m_vertexStride;
+
+ numfaces = m_numTriangleIndices;
+ (*indexbase) = (unsigned char *)m_triangleIndexBase;
+ indexstride = m_triangleIndexStride;
+ indicestype = PHY_INTEGER;
+}
+
diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.h b/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.h
new file mode 100644
index 00000000000..4784dc3938f
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2006 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+
+#include "StridingMeshInterface.h"
+
+
+class TriangleIndexVertexArray : public StridingMeshInterface
+{
+
+ int m_numTriangleIndices;
+ int* m_triangleIndexBase;
+ int m_triangleIndexStride;
+ int m_numVertices;
+ float* m_vertexBase;
+ int m_vertexStride;
+
+public:
+
+ TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride);
+
+ virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0);
+
+ virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const;
+
+ /// unLockVertexBase finishes the access to a subpart of the triangle mesh
+ /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
+ virtual void unLockVertexBase(int subpart) {}
+
+ virtual void unLockReadOnlyVertexBase(int subpart) const {}
+
+ /// getNumSubParts returns the number of seperate subparts
+ /// each subpart has a continuous array of vertices and indices
+ virtual int getNumSubParts() const { return 1;}
+
+ virtual void preallocateVertices(int numverts){}
+ virtual void preallocateIndices(int numindices){}
+
+}; \ No newline at end of file
diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleMesh.cpp b/extern/bullet/Bullet/CollisionShapes/TriangleMesh.cpp
index d02b9600985..38cccec3e15 100644
--- a/extern/bullet/Bullet/CollisionShapes/TriangleMesh.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/TriangleMesh.cpp
@@ -33,7 +33,24 @@ void TriangleMesh::getLockedVertexIndexBase(unsigned char **vertexbase, int& num
}
-int TriangleMesh::getNumSubParts()
+void TriangleMesh::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const
+{
+ numverts = 3;
+ *vertexbase = (unsigned char*)&m_triangles[subpart];
+ type = PHY_FLOAT;
+ stride = sizeof(SimdVector3);
+
+
+ numfaces = 1;
+ *indexbase = (unsigned char*) &myindices[0];
+ indicestype = PHY_INTEGER;
+ indexstride = sizeof(int);
+
+}
+
+
+
+int TriangleMesh::getNumSubParts() const
{
return m_triangles.size();
}
diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleMesh.h b/extern/bullet/Bullet/CollisionShapes/TriangleMesh.h
index 0b0b937bf12..891ef6ea91a 100644
--- a/extern/bullet/Bullet/CollisionShapes/TriangleMesh.h
+++ b/extern/bullet/Bullet/CollisionShapes/TriangleMesh.h
@@ -46,13 +46,17 @@ class TriangleMesh : public StridingMeshInterface
virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0);
+ virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const;
+
/// unLockVertexBase finishes the access to a subpart of the triangle mesh
/// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
virtual void unLockVertexBase(int subpart) {}
+ virtual void unLockReadOnlyVertexBase(int subpart) const {}
+
/// getNumSubParts returns the number of seperate subparts
/// each subpart has a continuous array of vertices and indices
- virtual int getNumSubParts();
+ virtual int getNumSubParts() const;
virtual void preallocateVertices(int numverts){}
virtual void preallocateIndices(int numindices){}
diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp
index 0c20f78c1df..743e413307e 100644
--- a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp
@@ -13,7 +13,7 @@
#include "SimdQuaternion.h"
#include "StridingMeshInterface.h"
#include "AabbUtil2.h"
-#include "NarrowPhaseCollision/CollisionMargin.h"
+#include "CollisionShapes/CollisionMargin.h"
#include "stdio.h"
@@ -21,8 +21,10 @@ TriangleMeshShape::TriangleMeshShape(StridingMeshInterface* meshInterface)
: m_meshInterface(meshInterface),
m_collisionMargin(CONVEX_DISTANCE_MARGIN)
{
+ RecalcLocalAabb();
}
+
TriangleMeshShape::~TriangleMeshShape()
{
@@ -34,23 +36,39 @@ TriangleMeshShape::~TriangleMeshShape()
void TriangleMeshShape::GetAabb(const SimdTransform& trans,SimdVector3& aabbMin,SimdVector3& aabbMax) const
{
+ SimdVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin);
+ SimdVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin);
+
+ SimdMatrix3x3 abs_b = trans.getBasis().absolute();
+
+ SimdPoint3 center = trans(localCenter);
+
+ SimdVector3 extent = SimdVector3(abs_b[0].dot(localHalfExtents),
+ abs_b[1].dot(localHalfExtents),
+ abs_b[2].dot(localHalfExtents));
+ extent += SimdVector3(GetMargin(),GetMargin(),GetMargin());
+
+ aabbMin = center - extent;
+ aabbMax = center + extent;
+
+
+}
+
+void TriangleMeshShape::RecalcLocalAabb()
+{
for (int i=0;i<3;i++)
{
SimdVector3 vec(0.f,0.f,0.f);
vec[i] = 1.f;
- SimdVector3 tmp = trans(LocalGetSupportingVertex(vec*trans.getBasis()));
- aabbMax[i] = tmp[i]+m_collisionMargin;
+ SimdVector3 tmp = LocalGetSupportingVertex(vec);
+ m_localAabbMax[i] = tmp[i]+m_collisionMargin;
vec[i] = -1.f;
- tmp = trans(LocalGetSupportingVertex(vec*trans.getBasis()));
- aabbMin[i] = tmp[i]-m_collisionMargin;
+ tmp = LocalGetSupportingVertex(vec);
+ m_localAabbMin[i] = tmp[i]-m_collisionMargin;
}
}
-TriangleCallback::~TriangleCallback()
-{
-
-}
class SupportVertexCallback : public TriangleCallback
{
@@ -98,6 +116,7 @@ public:
void TriangleMeshShape::setLocalScaling(const SimdVector3& scaling)
{
m_meshInterface->setScaling(scaling);
+ RecalcLocalAabb();
}
const SimdVector3& TriangleMeshShape::getLocalScaling() const
@@ -107,57 +126,43 @@ const SimdVector3& TriangleMeshShape::getLocalScaling() const
+
+
+
+//#define DEBUG_TRIANGLE_MESH
+
+
void TriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
{
- SimdVector3 meshScaling = m_meshInterface->getScaling();
- int numtotalphysicsverts = 0;
- int part,graphicssubparts = m_meshInterface->getNumSubParts();
- for (part=0;part<graphicssubparts ;part++)
+ struct FilteredCallback : public InternalTriangleIndexCallback
{
- unsigned char * vertexbase;
- unsigned char * indexbase;
- int indexstride;
- PHY_ScalarType type;
- PHY_ScalarType gfxindextype;
- int stride,numverts,numtriangles;
- m_meshInterface->getLockedVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
- numtotalphysicsverts+=numtriangles*3; //upper bound
-
-
- int gfxindex;
- SimdVector3 triangle[3];
-
- for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
+ TriangleCallback* m_callback;
+ SimdVector3 m_aabbMin;
+ SimdVector3 m_aabbMax;
+
+ FilteredCallback(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax)
+ :m_callback(callback),
+ m_aabbMin(aabbMin),
+ m_aabbMax(aabbMax)
{
-
- int graphicsindex=0;
-
- for (int j=2;j>=0;j--)
- {
- ASSERT(gfxindextype == PHY_INTEGER);
- int* gfxbase = (int*)(indexbase+gfxindex*indexstride);
- graphicsindex = gfxbase[j];
- float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
-
- triangle[j] = SimdVector3(
- graphicsbase[0]*meshScaling.getX(),
- graphicsbase[1]*meshScaling.getY(),
- graphicsbase[2]*meshScaling.getZ());
- }
+ }
- if (TestTriangleAgainstAabb2(&triangle[0],aabbMin,aabbMax))
+ virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex)
+ {
+ if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax))
{
//check aabb in triangle-space, before doing this
- callback->ProcessTriangle(triangle);
-
+ m_callback->ProcessTriangle(triangle);
}
}
-
- m_meshInterface->unLockVertexBase(part);
- }
+ };
+
+ FilteredCallback filterCallback(callback,aabbMin,aabbMax);
+
+ m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax);
}
diff --git a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h
index 926191bddbb..4f42458f062 100644
--- a/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h
+++ b/extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h
@@ -22,8 +22,10 @@
///Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
class TriangleMeshShape : public CollisionShape
{
-
+protected:
StridingMeshInterface* m_meshInterface;
+ SimdVector3 m_localAabbMin;
+ SimdVector3 m_localAabbMax;
float m_collisionMargin;
public:
@@ -41,6 +43,8 @@ public:
return LocalGetSupportingVertex(vec);
}
+ void RecalcLocalAabb();
+
virtual int GetShapeType() const
{
return TRIANGLE_MESH_SHAPE_PROXYTYPE;
@@ -48,7 +52,7 @@ public:
virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const;
- void ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
+ virtual void ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const;
virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.h b/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.h
index a1bcc615010..3d3c5de4110 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.h
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.h
@@ -13,7 +13,8 @@
#ifndef GJK_CONVEX_CAST_H
#define GJK_CONVEX_CAST_H
-#include "CollisionMargin.h"
+#include <CollisionShapes/CollisionMargin.h>
+
#include "SimdVector3.h"
#include "ConvexCast.h"
class ConvexShape;
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h
index f6311a9eba5..bfef4a91682 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h
@@ -17,7 +17,7 @@
#include "DiscreteCollisionDetectorInterface.h"
#include "SimdPoint3.h"
-#include "CollisionMargin.h"
+#include <CollisionShapes/CollisionMargin.h>
class ConvexShape;
#include "SimplexSolverInterface.h"
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.cpp
new file mode 100644
index 00000000000..bb45e5215f3
--- /dev/null
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.cpp
@@ -0,0 +1,33 @@
+
+#include "ManifoldContactAddResult.h"
+#include "NarrowPhaseCollision/PersistentManifold.h"
+
+ManifoldContactAddResult::ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr)
+ :m_manifoldPtr(manifoldPtr)
+{
+ m_transAInv = transA.inverse();
+ m_transBInv = transB.inverse();
+
+}
+
+
+void ManifoldContactAddResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+{
+ if (depth > m_manifoldPtr->GetManifoldMargin())
+ return;
+
+
+ SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth;
+ SimdVector3 localA = m_transAInv(pointA );
+ SimdVector3 localB = m_transBInv(pointInWorld);
+ ManifoldPoint newPt(localA,localB,normalOnBInWorld,depth);
+
+ int insertIndex = m_manifoldPtr->GetCacheEntry(newPt);
+ if (insertIndex >= 0)
+ {
+ m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex);
+ } else
+ {
+ m_manifoldPtr->AddManifoldPoint(newPt);
+ }
+} \ No newline at end of file
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.h b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.h
new file mode 100644
index 00000000000..1d019f97cd0
--- /dev/null
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability
+ * of this software for any purpose.
+ * It is provided "as is" without express or implied warranty.
+*/
+
+#ifndef MANIFOLD_CONTACT_ADD_RESULT_H
+#define MANIFOLD_CONTACT_ADD_RESULT_H
+
+#include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h"
+class PersistentManifold;
+
+class ManifoldContactAddResult : public DiscreteCollisionDetectorInterface::Result
+{
+ PersistentManifold* m_manifoldPtr;
+ SimdTransform m_transAInv;
+ SimdTransform m_transBInv;
+
+public:
+
+ ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr);
+
+ virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth);
+
+};
+
+#endif //MANIFOLD_CONTACT_ADD_RESULT_H
diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp
index 1926501b84b..6ff503e7322 100644
--- a/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp
+++ b/extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp
@@ -131,7 +131,7 @@ bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simpl
if (res.m_hasResult)
{
- pa = res.m_pointInWorld - res.m_normalOnBInWorld*res.m_depth;
+ pa = res.m_pointInWorld - res.m_normalOnBInWorld*0.1f*res.m_depth;
pb = res.m_pointInWorld;
}
return res.m_hasResult;
diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp
index 3f356c703d7..fef5ef281b2 100644
--- a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp
+++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp
@@ -38,7 +38,7 @@
///It improves the penetration depth handling dramatically
//#define USE_EPA
#ifdef USE_EPA
-#include "NarrowPhaseCollision/Solid3EpaPenetrationDepth.h"
+#include "../Extras/ExtraSolid35/Solid3EpaPenetrationDepth.h"
bool gUseEpa = true;
#else
bool gUseEpa = false;
@@ -68,11 +68,10 @@ ConvexConvexAlgorithm::ConvexConvexAlgorithm(PersistentManifold* mf,const Collis
m_gjkPairDetector(0,0,&m_simplexSolver,0),
m_box0(*proxy0),
m_box1(*proxy1),
-m_collisionImpulse(0.f),
m_ownManifold (false),
m_manifoldPtr(mf),
m_lowLevelOfDetail(false),
-m_useEpa(gUseEpa)
+m_useEpa(!gUseEpa)
{
CheckPenetrationDepthSolver();
@@ -137,30 +136,34 @@ public:
};
+static MinkowskiPenetrationDepthSolver gPenetrationDepthSolver;
+
+#ifdef USE_EPA
+Solid3EpaPenetrationDepth gSolidEpaPenetrationSolver;
+#endif //USE_EPA
+
void ConvexConvexAlgorithm::CheckPenetrationDepthSolver()
{
-// if (m_useEpa != gUseEpa)
+ if (m_useEpa != gUseEpa)
{
m_useEpa = gUseEpa;
if (m_useEpa)
{
//not distributed, see top of this file
#ifdef USE_EPA
- m_gjkPairDetector.SetPenetrationDepthSolver(new Solid3EpaPenetrationDepth);
+ m_gjkPairDetector.SetPenetrationDepthSolver(&gSolidEpaPenetrationSolver);
#else
- m_gjkPairDetector.SetPenetrationDepthSolver(new MinkowskiPenetrationDepthSolver);
+ m_gjkPairDetector.SetPenetrationDepthSolver(&gPenetrationDepthSolver);
#endif
} else
{
- m_gjkPairDetector.SetPenetrationDepthSolver(new MinkowskiPenetrationDepthSolver);
+ m_gjkPairDetector.SetPenetrationDepthSolver(&gPenetrationDepthSolver);
}
}
}
-bool extra = false;
-float gFriction = 0.5f;
//
// box-box collision algorithm, for simplicity also applies resolution-impulse
//
@@ -169,7 +172,7 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy
CheckPenetrationDepthSolver();
// printf("ConvexConvexAlgorithm::ProcessCollision\n");
-m_collisionImpulse = 0.f;
+
RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
RigidBody* body1 = (RigidBody*)m_box1.m_clientObject;
@@ -227,7 +230,7 @@ float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,Broad
CheckPenetrationDepthSolver();
- m_collisionImpulse = 0.f;
+
RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
RigidBody* body1 = (RigidBody*)m_box1.m_clientObject;
diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.h b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.h
index 8b16687f192..b560e8d4c15 100644
--- a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.h
+++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.h
@@ -29,7 +29,7 @@ class ConvexConvexAlgorithm : public CollisionAlgorithm
public:
BroadphaseProxy m_box0;
BroadphaseProxy m_box1;
- float m_collisionImpulse;
+
bool m_ownManifold;
PersistentManifold* m_manifoldPtr;
bool m_lowLevelOfDetail;
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
index 01c2dbf031d..be6793dc192 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp
@@ -139,28 +139,31 @@ SimdScalar rel_vel;
*/
rel_vel = normal.dot(vel);
-// if (rel_vel< 0.f)//-SIMD_EPSILON)
-// {
float combinedRestitution = body1.getRestitution() * body2.getRestitution();
- SimdScalar rest = restitutionCurve(rel_vel, combinedRestitution);
-// SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
+ SimdScalar restitution = restitutionCurve(rel_vel, combinedRestitution);
- SimdScalar timeCorrection = 0.5f / solverInfo.m_timeStep ;
+ SimdScalar Kfps = 1.f / solverInfo.m_timeStep ;
float damping = solverInfo.m_damping ;
- float tau = solverInfo.m_tau;
-
+ float Kerp = solverInfo.m_tau;
+
if (useGlobalSettingContacts)
{
damping = contactDamping;
- tau = contactTau;
+ Kerp = contactTau;
}
- SimdScalar penetrationImpulse = (-distance* tau *timeCorrection) * jacDiagABInv;
-
+ float Kcor = Kerp *Kfps;
+
+
+ SimdScalar positionalError = Kcor *-distance;
+ //jacDiagABInv;
+ SimdScalar velocityError = -(1.0f + restitution) * damping * rel_vel;
+
+ SimdScalar penetrationImpulse = positionalError * jacDiagABInv;
- SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel * jacDiagABInv;
+ SimdScalar velocityImpulse = velocityError * jacDiagABInv;
SimdScalar friction_impulse = 0.f;
@@ -177,11 +180,11 @@ SimdScalar rel_vel;
{
- SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
- SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
- SimdVector3 vel = vel1 - vel2;
-
- rel_vel = normal.dot(vel);
+ SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+ SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+ SimdVector3 vel = vel1 - vel2;
+
+ rel_vel = normal.dot(vel);
#define PER_CONTACT_FRICTION
#ifdef PER_CONTACT_FRICTION
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h b/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h
index ebf58c34485..3a89f2ad1fd 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h
@@ -16,7 +16,7 @@
//notes:
-// Another memory optimization would be to store m_MbJ in the remaining 3 w components
+// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
// which makes the JacobianEntry memory layout 16 bytes
// if you only are interested in angular part, just feed massInvA and massInvB zero
@@ -32,59 +32,59 @@ public:
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
- const SimdVector3& normal,
+ const SimdVector3& jointAxis,
const SimdVector3& inertiaInvA,
const SimdScalar massInvA,
const SimdVector3& inertiaInvB,
const SimdScalar massInvB)
- :m_normalAxis(normal)
+ :m_jointAxis(jointAxis)
{
- m_aJ = world2A*(rel_pos1.cross(normal));
- m_bJ = world2B*(rel_pos2.cross(normal));
- m_MaJ = inertiaInvA * m_aJ;
- m_MbJ = inertiaInvB * m_bJ;
- m_jacDiagAB = massInvA + m_MaJ.dot(m_aJ) + massInvB + m_MbJ.dot(m_bJ);
+ m_aJ = world2A*(rel_pos1.cross(m_jointAxis));
+ m_bJ = world2B*(rel_pos2.cross(m_jointAxis));
+ m_0MinvJt = inertiaInvA * m_aJ;
+ m_1MinvJt = inertiaInvB * m_bJ;
+ m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
}
//angular constraint between two different rigidbodies
- JacobianEntry(const SimdVector3& normal,
+ JacobianEntry(const SimdVector3& jointAxis,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const SimdVector3& inertiaInvA,
const SimdVector3& inertiaInvB)
- :m_normalAxis(normal)
+ :m_jointAxis(m_jointAxis)
{
- m_aJ= world2A*normal;
- m_bJ = world2B*-normal;
- m_MaJ = inertiaInvA * m_aJ;
- m_MbJ = inertiaInvB * m_bJ;
- m_jacDiagAB = m_MaJ.dot(m_aJ) + m_MbJ.dot(m_bJ);
+ m_aJ= world2A*m_jointAxis;
+ m_bJ = world2B*-m_jointAxis;
+ m_0MinvJt = inertiaInvA * m_aJ;
+ m_1MinvJt = inertiaInvB * m_bJ;
+ m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
}
//constraint on one rigidbody
JacobianEntry(
const SimdMatrix3x3& world2A,
const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
- const SimdVector3& normal,
+ const SimdVector3& jointAxis,
const SimdVector3& inertiaInvA,
const SimdScalar massInvA)
- :m_normalAxis(normal)
+ :m_jointAxis(jointAxis)
{
- m_aJ= world2A*(rel_pos1.cross(normal));
- m_bJ = world2A*(rel_pos2.cross(normal));
- m_MaJ = inertiaInvA * m_aJ;
- m_MbJ = SimdVector3(0.f,0.f,0.f);
- m_jacDiagAB = massInvA + m_MaJ.dot(m_aJ);
+ m_aJ= world2A*(rel_pos1.cross(m_jointAxis));
+ m_bJ = world2A*(rel_pos2.cross(m_jointAxis));
+ m_0MinvJt = inertiaInvA * m_aJ;
+ m_1MinvJt = SimdVector3(0.f,0.f,0.f);
+ m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
}
- SimdScalar getDiagonal() const { return m_jacDiagAB; }
+ SimdScalar getDiagonal() const { return m_Adiag; }
// for two constraints on the same rigidbody (for example vehicle friction)
SimdScalar getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const
{
const JacobianEntry& jacA = *this;
- SimdScalar lin = massInvA * jacA.m_normalAxis.dot(jacB.m_normalAxis);
- SimdScalar ang = jacA.m_MaJ.dot(jacB.m_aJ);
+ SimdScalar lin = massInvA * jacA.m_jointAxis.dot(jacB.m_jointAxis);
+ SimdScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
return lin + ang;
}
@@ -94,9 +94,9 @@ public:
SimdScalar getNonDiagonal(const JacobianEntry& jacB,const SimdScalar massInvA,const SimdScalar massInvB) const
{
const JacobianEntry& jacA = *this;
- SimdVector3 lin = jacA.m_normalAxis * jacB.m_normalAxis;
- SimdVector3 ang0 = jacA.m_MaJ * jacB.m_aJ;
- SimdVector3 ang1 = jacA.m_MbJ * jacB.m_bJ;
+ SimdVector3 lin = jacA.m_jointAxis* jacB.m_jointAxis;
+ SimdVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
+ SimdVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
SimdVector3 lin0 = massInvA * lin ;
SimdVector3 lin1 = massInvB * lin;
SimdVector3 sum = ang0+ang1+lin0+lin1;
@@ -108,7 +108,7 @@ public:
SimdVector3 linrel = linvelA - linvelB;
SimdVector3 angvela = angvelA * m_aJ;
SimdVector3 angvelb = angvelB * m_bJ;
- linrel *= m_normalAxis;
+ linrel *= m_jointAxis;
angvela += angvelb;
angvela += linrel;
SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
@@ -116,13 +116,13 @@ public:
}
//private:
- SimdVector3 m_normalAxis;
+ SimdVector3 m_jointAxis;
SimdVector3 m_aJ;
SimdVector3 m_bJ;
- SimdVector3 m_MaJ;
- SimdVector3 m_MbJ;
+ SimdVector3 m_0MinvJt;
+ SimdVector3 m_1MinvJt;
//Optimization: can be stored in the w/last component of one of the vectors
- SimdScalar m_jacDiagAB;
+ SimdScalar m_Adiag;
};
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp
index 1fb23b40752..acb6728deeb 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp
@@ -49,7 +49,7 @@ class BU_Joint;
OdeConstraintSolver::OdeConstraintSolver():
m_cfm(1e-5f),
-m_erp(0.3f)
+m_erp(0.4f)
{
}
diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp
index a6c0fda96f3..f7c31dd0d68 100644
--- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp
+++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp
@@ -58,11 +58,6 @@ void RigidBody::getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const
void RigidBody::SetCollisionShape(CollisionShape* mink)
{
m_collisionShape = mink;
- SimdTransform ident;
- ident.setIdentity();
- SimdVector3 aabbMin,aabbMax;
- m_collisionShape ->GetAabb(ident,aabbMin,aabbMax);
- SimdVector3 diag = (aabbMax-aabbMin)*0.5f;
}
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
index 5ddacd05d05..919d1f9e58c 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
@@ -39,10 +39,12 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
MassProps mp(ci.m_mass, ci.m_localInertiaTensor);
m_body = new RigidBody(mp,0,0,ci.m_friction,ci.m_restitution);
+
+ m_body->SetCollisionShape( ci.m_collisionShape);
+
m_broadphaseHandle = ci.m_broadphaseHandle;
- m_collisionShape = ci.m_collisionShape;
//
// init the rigidbody properly
@@ -67,7 +69,6 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
CcdPhysicsController::~CcdPhysicsController()
{
//will be reference counted, due to sharing
- //delete m_collisionShape;
delete m_MotionState;
delete m_body;
}
@@ -89,12 +90,18 @@ bool CcdPhysicsController::SynchronizeMotionStates(float time)
m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
SimdVector3 scaling(scale[0],scale[1],scale[2]);
- m_collisionShape->setLocalScaling(scaling);
-
+ m_body->GetCollisionShape()->setLocalScaling(scaling);
return true;
}
+CollisionShape* CcdPhysicsController::GetCollisionShape()
+{
+ return m_body->GetCollisionShape();
+}
+
+
+
/**
WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
*/
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h
index 4f189b7f324..478e0e66a39 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h
@@ -53,7 +53,6 @@ class CcdPhysicsController : public PHY_IPhysicsController
{
RigidBody* m_body;
class PHY_IMotionState* m_MotionState;
- CollisionShape* m_collisionShape;
void* m_newClientInfo;
void GetWorldOrientation(SimdMatrix3x3& mat);
@@ -71,7 +70,7 @@ class CcdPhysicsController : public PHY_IPhysicsController
RigidBody* GetRigidBody() { return m_body;}
- CollisionShape* GetCollisionShape() { return m_collisionShape;}
+ CollisionShape* GetCollisionShape();
////////////////////////////////////
// PHY_IPhysicsController interface
////////////////////////////////////
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
index b68e730b4c6..558f294344a 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
@@ -118,6 +118,7 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
BroadphaseInterface* scene = m_broadphase;
+
CollisionShape* shapeinterface = ctrl->GetCollisionShape();
assert(shapeinterface);
@@ -171,7 +172,7 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
maxAabb);
}
- body->SetCollisionShape( shapeinterface );
+
diff --git a/extern/bullet/LinearMath/SimdQuadWord.h b/extern/bullet/LinearMath/SimdQuadWord.h
index 492981fb303..a9967682d61 100644
--- a/extern/bullet/LinearMath/SimdQuadWord.h
+++ b/extern/bullet/LinearMath/SimdQuadWord.h
@@ -109,6 +109,38 @@ class SimdQuadWord
}
+ SIMD_FORCE_INLINE void setMax(const SimdQuadWord& other)
+ {
+ if (other.m_x > m_x)
+ m_x = other.m_x;
+
+ if (other.m_y > m_y)
+ m_y = other.m_y;
+
+ if (other.m_z > m_z)
+ m_z = other.m_z;
+
+ if (other.m_unusedW > m_unusedW)
+ m_unusedW = other.m_unusedW;
+ }
+
+ SIMD_FORCE_INLINE void setMin(const SimdQuadWord& other)
+ {
+ if (other.m_x < m_x)
+ m_x = other.m_x;
+
+ if (other.m_y < m_y)
+ m_y = other.m_y;
+
+ if (other.m_z < m_z)
+ m_z = other.m_z;
+
+ if (other.m_unusedW < m_unusedW)
+ m_unusedW = other.m_unusedW;
+ }
+
+
+
};
#endif //SIMD_QUADWORD_H
diff --git a/extern/bullet/LinearMath/SimdScalar.h b/extern/bullet/LinearMath/SimdScalar.h
index 4ea359db1b6..f20de0d06d9 100644
--- a/extern/bullet/LinearMath/SimdScalar.h
+++ b/extern/bullet/LinearMath/SimdScalar.h
@@ -39,7 +39,8 @@ DEALINGS IN THE SOFTWARE.
#include <float.h>
#ifdef WIN32
-
+#pragma warning(disable:4530)
+#pragma warning(disable:4996)
#ifdef __MINGW32__
#define SIMD_FORCE_INLINE inline
#else
@@ -59,8 +60,6 @@ DEALINGS IN THE SOFTWARE.
#endif
-
-
#define ASSERT assert
#endif
@@ -119,6 +118,16 @@ SIMD_FORCE_INLINE bool SimdEqual(SimdScalar a, SimdScalar eps) {
SIMD_FORCE_INLINE bool SimdGreaterEqual (SimdScalar a, SimdScalar eps) {
return (!((a) <= eps));
}
+
+/*SIMD_FORCE_INLINE SimdScalar SimdCos(SimdScalar x) { return cosf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdSin(SimdScalar x) { return sinf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdTan(SimdScalar x) { return tanf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAcos(SimdScalar x) { return acosf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAsin(SimdScalar x) { return asinf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAtan(SimdScalar x) { return atanf(x); }
+SIMD_FORCE_INLINE SimdScalar SimdAtan2(SimdScalar x, SimdScalar y) { return atan2f(x, y); }
+*/
+
SIMD_FORCE_INLINE int SimdSign(SimdScalar x) {
return x < 0.0f ? -1 : x > 0.0f ? 1 : 0;
}
diff --git a/extern/bullet/SConscript b/extern/bullet/SConscript
index ae3763d6e45..453d18aff6d 100644
--- a/extern/bullet/SConscript
+++ b/extern/bullet/SConscript
@@ -57,7 +57,13 @@ bullet_sources = ['Bullet/BroadphaseCollision/BroadphaseProxy.cpp',
'Bullet/CollisionShapes/StridingMeshInterface.cpp',
'Bullet/CollisionShapes/TriangleMesh.cpp',
'Bullet/CollisionShapes/TriangleMeshShape.cpp',
-
+ 'Bullet/CollisionShapes/BvhTriangleMeshShape.cpp',
+ 'Bullet/CollisionShapes/ConvexTriangleCallback.cpp',
+ 'Bullet/CollisionShapes/EmptyShape.cpp',
+ 'Bullet/CollisionShapes/OptimizedBvh.cpp',
+ 'Bullet/CollisionShapes/TriangleCallback.cpp',
+ 'Bullet/CollisionShapes/TriangleIndexVertexArray.cpp',
+
'Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp',
'Bullet/NarrowPhaseCollision/BU_Collidable.cpp',
'Bullet/NarrowPhaseCollision/BU_CollisionPair.cpp',
@@ -73,7 +79,8 @@ bullet_sources = ['Bullet/BroadphaseCollision/BroadphaseProxy.cpp',
'Bullet/NarrowPhaseCollision/RaycastCallback.cpp',
'Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp',
'Bullet/NarrowPhaseCollision/VoronoiSimplexSolver.cpp',
-
+ 'Bullet/NarrowPhaseCollision/ManifoldContactAddResult.cpp',
+
'BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp',
'BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp',
'BulletDynamics/CollisionDispatch/EmptyCollisionAlgorithm.cpp',