diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionShapes')
80 files changed, 5618 insertions, 4608 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp index ecce028c2e6..a3d8075dafa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp @@ -15,28 +15,23 @@ subject to the following restrictions: #include "btBox2dShape.h" +//{ -//{ - - -void btBox2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btBox2dShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); + btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax); } - -void btBox2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btBox2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //btScalar margin = btScalar(0.); btVector3 halfExtents = getHalfExtentsWithMargin(); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h index ce333783e44..7e085f9e2ea 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h @@ -23,9 +23,9 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" ///The btBox2dShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space. -ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) +btBox2dShape : public btPolyhedralConvexShape { - //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead btVector3 m_centroid; @@ -33,122 +33,111 @@ ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape btVector3 m_normals[4]; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 getHalfExtentsWithMargin() const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; return halfExtents; } - + const btVector3& getHalfExtentsWithoutMargin() const { - return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included + return m_implicitShapeDimensions; //changed in Bullet 2.63: assume the scaling and margin are included } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; - + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - - for (int i=0;i<numVectors;i++) + + for (int i = 0; i < numVectors; i++) { const btVector3& vec = vectors[i]; supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - } - ///a btBox2dShape is a flat 2D box in the X-Y plane (Z extents are zero) - btBox2dShape( const btVector3& boxHalfExtents) + btBox2dShape(const btVector3& boxHalfExtents) : btPolyhedralConvexShape(), - m_centroid(0,0,0) + m_centroid(0, 0, 0) { - m_vertices[0].setValue(-boxHalfExtents.getX(),-boxHalfExtents.getY(),0); - m_vertices[1].setValue(boxHalfExtents.getX(),-boxHalfExtents.getY(),0); - m_vertices[2].setValue(boxHalfExtents.getX(),boxHalfExtents.getY(),0); - m_vertices[3].setValue(-boxHalfExtents.getX(),boxHalfExtents.getY(),0); + m_vertices[0].setValue(-boxHalfExtents.getX(), -boxHalfExtents.getY(), 0); + m_vertices[1].setValue(boxHalfExtents.getX(), -boxHalfExtents.getY(), 0); + m_vertices[2].setValue(boxHalfExtents.getX(), boxHalfExtents.getY(), 0); + m_vertices[3].setValue(-boxHalfExtents.getX(), boxHalfExtents.getY(), 0); - m_normals[0].setValue(0,-1,0); - m_normals[1].setValue(1,0,0); - m_normals[2].setValue(0,1,0); - m_normals[3].setValue(-1,0,0); + m_normals[0].setValue(0, -1, 0); + m_normals[1].setValue(1, 0, 0); + m_normals[2].setValue(0, 1, 0); + m_normals[3].setValue(-1, 0, 0); btScalar minDimension = boxHalfExtents.getX(); - if (minDimension>boxHalfExtents.getY()) + if (minDimension > boxHalfExtents.getY()) minDimension = boxHalfExtents.getY(); - setSafeMargin(minDimension); m_shapeType = BOX_2D_SHAPE_PROXYTYPE; - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; + + setSafeMargin(minDimension); }; virtual void setMargin(btScalar collisionMargin) { //correct the m_implicitShapeDimensions for the margin - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; - + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; + btConvexInternalShape::setMargin(collisionMargin); - btVector3 newMargin(getMargin(),getMargin(),getMargin()); + btVector3 newMargin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; - } - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; btConvexInternalShape::setLocalScaling(scaling); m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; - } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - - - - int getVertexCount() const + int getVertexCount() const { return 4; } - virtual int getNumVertices()const + virtual int getNumVertices() const { return 4; } @@ -163,82 +152,70 @@ public: return &m_normals[0]; } - - - - - - - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const { //this plane might not be aligned... - btVector4 plane ; - getPlaneEquation(plane,i); - planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ()); + btVector4 plane; + getPlaneEquation(plane, i); + planeNormal = btVector3(plane.getX(), plane.getY(), plane.getZ()); planeSupport = localGetSupportingVertex(-planeNormal); } - const btVector3& getCentroid() const { return m_centroid; } - + virtual int getNumPlanes() const { return 6; - } - - + } virtual int getNumEdges() const { return 12; } - - virtual void getVertex(int i,btVector3& vtx) const + virtual void getVertex(int i, btVector3& vtx) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); vtx = btVector3( - halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1), - halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1), - halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2)); + halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1), + halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1), + halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2)); } - - virtual void getPlaneEquation(btVector4& plane,int i) const + virtual void getPlaneEquation(btVector4 & plane, int i) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); switch (i) { - case 0: - plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 1: - plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 2: - plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y()); - break; - case 3: - plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y()); - break; - case 4: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z()); - break; - case 5: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z()); - break; - default: - btAssert(0); + case 0: + plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 1: + plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 2: + plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y()); + break; + case 3: + plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y()); + break; + case 4: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z()); + break; + case 5: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z()); + break; + default: + btAssert(0); } } - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const //virtual void getEdge(int i,Edge& edge) const { int edgeVert0 = 0; @@ -246,126 +223,117 @@ public: switch (i) { - case 0: + case 0: edgeVert0 = 0; edgeVert1 = 1; - break; - case 1: + break; + case 1: edgeVert0 = 0; edgeVert1 = 2; - break; - case 2: - edgeVert0 = 1; - edgeVert1 = 3; - - break; - case 3: - edgeVert0 = 2; - edgeVert1 = 3; - break; - case 4: - edgeVert0 = 0; - edgeVert1 = 4; - break; - case 5: - edgeVert0 = 1; - edgeVert1 = 5; - - break; - case 6: - edgeVert0 = 2; - edgeVert1 = 6; - break; - case 7: - edgeVert0 = 3; - edgeVert1 = 7; - break; - case 8: - edgeVert0 = 4; - edgeVert1 = 5; - break; - case 9: - edgeVert0 = 4; - edgeVert1 = 6; - break; - case 10: - edgeVert0 = 5; - edgeVert1 = 7; - break; - case 11: - edgeVert0 = 6; - edgeVert1 = 7; - break; - default: - btAssert(0); - + break; + case 2: + edgeVert0 = 1; + edgeVert1 = 3; + + break; + case 3: + edgeVert0 = 2; + edgeVert1 = 3; + break; + case 4: + edgeVert0 = 0; + edgeVert1 = 4; + break; + case 5: + edgeVert0 = 1; + edgeVert1 = 5; + + break; + case 6: + edgeVert0 = 2; + edgeVert1 = 6; + break; + case 7: + edgeVert0 = 3; + edgeVert1 = 7; + break; + case 8: + edgeVert0 = 4; + edgeVert1 = 5; + break; + case 9: + edgeVert0 = 4; + edgeVert1 = 6; + break; + case 10: + edgeVert0 = 5; + edgeVert1 = 7; + break; + case 11: + edgeVert0 = 6; + edgeVert1 = 7; + break; + default: + btAssert(0); } - getVertex(edgeVert0,pa ); - getVertex(edgeVert1,pb ); + getVertex(edgeVert0, pa); + getVertex(edgeVert1, pb); } - - - - - virtual bool isInside(const btVector3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt, btScalar tolerance) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); //btScalar minDist = 2*tolerance; - - bool result = (pt.x() <= (halfExtents.x()+tolerance)) && - (pt.x() >= (-halfExtents.x()-tolerance)) && - (pt.y() <= (halfExtents.y()+tolerance)) && - (pt.y() >= (-halfExtents.y()-tolerance)) && - (pt.z() <= (halfExtents.z()+tolerance)) && - (pt.z() >= (-halfExtents.z()-tolerance)); - + + bool result = (pt.x() <= (halfExtents.x() + tolerance)) && + (pt.x() >= (-halfExtents.x() - tolerance)) && + (pt.y() <= (halfExtents.y() + tolerance)) && + (pt.y() >= (-halfExtents.y() - tolerance)) && + (pt.z() <= (halfExtents.z() + tolerance)) && + (pt.z() >= (-halfExtents.z() - tolerance)); + return result; } - //debugging - virtual const char* getName()const + virtual const char* getName() const { return "Box2d"; } - virtual int getNumPreferredPenetrationDirections() const + virtual int getNumPreferredPenetrationDirections() const { return 6; } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { switch (index) { - case 0: - penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); - break; - case 1: - penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); - break; - case 2: - penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); - break; - case 3: - penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); - break; - case 4: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); - break; - case 5: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); - break; - default: - btAssert(0); + case 0: + penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.)); + break; + case 1: + penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.)); + break; + case 2: + penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.)); + break; + case 3: + penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.)); + break; + case 4: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.)); + break; + case 5: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.)); + break; + default: + btAssert(0); } } - }; -#endif //BT_OBB_BOX_2D_SHAPE_H - - +#endif //BT_OBB_BOX_2D_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp index 3859138f18a..cb91d023e40 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -14,38 +14,32 @@ subject to the following restrictions: */ #include "btBoxShape.h" -btBoxShape::btBoxShape( const btVector3& boxHalfExtents) -: btPolyhedralConvexShape() +btBoxShape::btBoxShape(const btVector3& boxHalfExtents) + : btPolyhedralConvexShape() { m_shapeType = BOX_SHAPE_PROXYTYPE; - setSafeMargin(boxHalfExtents); - - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; -}; - - + setSafeMargin(boxHalfExtents); +}; -void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btBoxShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); + btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax); } - -void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btBoxShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //btScalar margin = btScalar(0.); btVector3 halfExtents = getHalfExtentsWithMargin(); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h index 715e3f2ab98..3c65505d5bb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -23,112 +23,102 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" ///The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space. -ATTRIBUTE_ALIGNED16(class) btBoxShape: public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) +btBoxShape : public btPolyhedralConvexShape { - //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead - public: - -BT_DECLARE_ALIGNED_ALLOCATOR(); + BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 getHalfExtentsWithMargin() const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; return halfExtents; } - + const btVector3& getHalfExtentsWithoutMargin() const { - return m_implicitShapeDimensions;//scaling is included, margin is not + return m_implicitShapeDimensions; //scaling is included, margin is not } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; - + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + SIMD_FORCE_INLINE btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - + return btVector3(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - - for (int i=0;i<numVectors;i++) + + for (int i = 0; i < numVectors; i++) { const btVector3& vec = vectors[i]; supportVerticesOut[i].setValue(btFsels(vec.x(), halfExtents.x(), -halfExtents.x()), - btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), - btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); + btFsels(vec.y(), halfExtents.y(), -halfExtents.y()), + btFsels(vec.z(), halfExtents.z(), -halfExtents.z())); } - } - - btBoxShape( const btVector3& boxHalfExtents); + btBoxShape(const btVector3& boxHalfExtents); virtual void setMargin(btScalar collisionMargin) { //correct the m_implicitShapeDimensions for the margin - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; - + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; + btConvexInternalShape::setMargin(collisionMargin); - btVector3 newMargin(getMargin(),getMargin(),getMargin()); + btVector3 newMargin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; - } - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; btConvexInternalShape::setLocalScaling(scaling); m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; - } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const { //this plane might not be aligned... - btVector4 plane ; - getPlaneEquation(plane,i); - planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ()); + btVector4 plane; + getPlaneEquation(plane, i); + planeNormal = btVector3(plane.getX(), plane.getY(), plane.getZ()); planeSupport = localGetSupportingVertex(-planeNormal); } - virtual int getNumPlanes() const { return 6; - } - - virtual int getNumVertices() const + } + + virtual int getNumVertices() const { return 8; } @@ -138,49 +128,46 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); return 12; } - - virtual void getVertex(int i,btVector3& vtx) const + virtual void getVertex(int i, btVector3& vtx) const { btVector3 halfExtents = getHalfExtentsWithMargin(); vtx = btVector3( - halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1), - halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1), - halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2)); + halfExtents.x() * (1 - (i & 1)) - halfExtents.x() * (i & 1), + halfExtents.y() * (1 - ((i & 2) >> 1)) - halfExtents.y() * ((i & 2) >> 1), + halfExtents.z() * (1 - ((i & 4) >> 2)) - halfExtents.z() * ((i & 4) >> 2)); } - - virtual void getPlaneEquation(btVector4& plane,int i) const + virtual void getPlaneEquation(btVector4 & plane, int i) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); switch (i) { - case 0: - plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 1: - plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x()); - break; - case 2: - plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y()); - break; - case 3: - plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y()); - break; - case 4: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z()); - break; - case 5: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z()); - break; - default: - btAssert(0); + case 0: + plane.setValue(btScalar(1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 1: + plane.setValue(btScalar(-1.), btScalar(0.), btScalar(0.), -halfExtents.x()); + break; + case 2: + plane.setValue(btScalar(0.), btScalar(1.), btScalar(0.), -halfExtents.y()); + break; + case 3: + plane.setValue(btScalar(0.), btScalar(-1.), btScalar(0.), -halfExtents.y()); + break; + case 4: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(1.), -halfExtents.z()); + break; + case 5: + plane.setValue(btScalar(0.), btScalar(0.), btScalar(-1.), -halfExtents.z()); + break; + default: + btAssert(0); } } - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const //virtual void getEdge(int i,Edge& edge) const { int edgeVert0 = 0; @@ -188,127 +175,117 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); switch (i) { - case 0: + case 0: edgeVert0 = 0; edgeVert1 = 1; - break; - case 1: + break; + case 1: edgeVert0 = 0; edgeVert1 = 2; - break; - case 2: - edgeVert0 = 1; - edgeVert1 = 3; - - break; - case 3: - edgeVert0 = 2; - edgeVert1 = 3; - break; - case 4: - edgeVert0 = 0; - edgeVert1 = 4; - break; - case 5: - edgeVert0 = 1; - edgeVert1 = 5; - - break; - case 6: - edgeVert0 = 2; - edgeVert1 = 6; - break; - case 7: - edgeVert0 = 3; - edgeVert1 = 7; - break; - case 8: - edgeVert0 = 4; - edgeVert1 = 5; - break; - case 9: - edgeVert0 = 4; - edgeVert1 = 6; - break; - case 10: - edgeVert0 = 5; - edgeVert1 = 7; - break; - case 11: - edgeVert0 = 6; - edgeVert1 = 7; - break; - default: - btAssert(0); - + break; + case 2: + edgeVert0 = 1; + edgeVert1 = 3; + + break; + case 3: + edgeVert0 = 2; + edgeVert1 = 3; + break; + case 4: + edgeVert0 = 0; + edgeVert1 = 4; + break; + case 5: + edgeVert0 = 1; + edgeVert1 = 5; + + break; + case 6: + edgeVert0 = 2; + edgeVert1 = 6; + break; + case 7: + edgeVert0 = 3; + edgeVert1 = 7; + break; + case 8: + edgeVert0 = 4; + edgeVert1 = 5; + break; + case 9: + edgeVert0 = 4; + edgeVert1 = 6; + break; + case 10: + edgeVert0 = 5; + edgeVert1 = 7; + break; + case 11: + edgeVert0 = 6; + edgeVert1 = 7; + break; + default: + btAssert(0); } - getVertex(edgeVert0,pa ); - getVertex(edgeVert1,pb ); + getVertex(edgeVert0, pa); + getVertex(edgeVert1, pb); } - - - - - virtual bool isInside(const btVector3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt, btScalar tolerance) const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); //btScalar minDist = 2*tolerance; - - bool result = (pt.x() <= (halfExtents.x()+tolerance)) && - (pt.x() >= (-halfExtents.x()-tolerance)) && - (pt.y() <= (halfExtents.y()+tolerance)) && - (pt.y() >= (-halfExtents.y()-tolerance)) && - (pt.z() <= (halfExtents.z()+tolerance)) && - (pt.z() >= (-halfExtents.z()-tolerance)); - + + bool result = (pt.x() <= (halfExtents.x() + tolerance)) && + (pt.x() >= (-halfExtents.x() - tolerance)) && + (pt.y() <= (halfExtents.y() + tolerance)) && + (pt.y() >= (-halfExtents.y() - tolerance)) && + (pt.z() <= (halfExtents.z() + tolerance)) && + (pt.z() >= (-halfExtents.z() - tolerance)); + return result; } - //debugging - virtual const char* getName()const + virtual const char* getName() const { return "Box"; } - virtual int getNumPreferredPenetrationDirections() const + virtual int getNumPreferredPenetrationDirections() const { return 6; } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { switch (index) { - case 0: - penetrationVector.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); - break; - case 1: - penetrationVector.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); - break; - case 2: - penetrationVector.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); - break; - case 3: - penetrationVector.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); - break; - case 4: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); - break; - case 5: - penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); - break; - default: - btAssert(0); + case 0: + penetrationVector.setValue(btScalar(1.), btScalar(0.), btScalar(0.)); + break; + case 1: + penetrationVector.setValue(btScalar(-1.), btScalar(0.), btScalar(0.)); + break; + case 2: + penetrationVector.setValue(btScalar(0.), btScalar(1.), btScalar(0.)); + break; + case 3: + penetrationVector.setValue(btScalar(0.), btScalar(-1.), btScalar(0.)); + break; + case 4: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(1.)); + break; + case 5: + penetrationVector.setValue(btScalar(0.), btScalar(0.), btScalar(-1.)); + break; + default: + btAssert(0); } } - }; - -#endif //BT_OBB_BOX_MINKOWSKI_H - - +#endif //BT_OBB_BOX_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index ace4cfa2646..c66ce58e3ef 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -22,11 +22,11 @@ subject to the following restrictions: ///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. btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh) -:btTriangleMeshShape(meshInterface), -m_bvh(0), -m_triangleInfoMap(0), -m_useQuantizedAabbCompression(useQuantizedAabbCompression), -m_ownsBvh(false) + : btTriangleMeshShape(meshInterface), + m_bvh(0), + m_triangleInfoMap(0), + m_useQuantizedAabbCompression(useQuantizedAabbCompression), + m_ownsBvh(false) { m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; //construct bvh from meshInterface @@ -37,16 +37,15 @@ m_ownsBvh(false) buildOptimizedBvh(); } -#endif //DISABLE_BVH - +#endif //DISABLE_BVH } -btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,bool buildBvh) -:btTriangleMeshShape(meshInterface), -m_bvh(0), -m_triangleInfoMap(0), -m_useQuantizedAabbCompression(useQuantizedAabbCompression), -m_ownsBvh(false) +btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh) + : btTriangleMeshShape(meshInterface), + m_bvh(0), + m_triangleInfoMap(0), + m_useQuantizedAabbCompression(useQuantizedAabbCompression), + m_ownsBvh(false) { m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; //construct bvh from meshInterface @@ -54,30 +53,28 @@ m_ownsBvh(false) if (buildBvh) { - void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16); + void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16); m_bvh = new (mem) btOptimizedBvh(); - - m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); + + m_bvh->build(meshInterface, m_useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax); m_ownsBvh = true; } -#endif //DISABLE_BVH - +#endif //DISABLE_BVH } -void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax) +void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax) { - m_bvh->refitPartial( m_meshInterface,aabbMin,aabbMax ); - + m_bvh->refitPartial(m_meshInterface, aabbMin, aabbMax); + m_localAabbMin.setMin(aabbMin); m_localAabbMax.setMax(aabbMax); } - -void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin,const btVector3& aabbMax) +void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin, const btVector3& aabbMax) { - m_bvh->refit( m_meshInterface, aabbMin,aabbMax ); - + m_bvh->refit(m_meshInterface, aabbMin, aabbMax); + recalcLocalAabb(); } @@ -90,27 +87,27 @@ btBvhTriangleMeshShape::~btBvhTriangleMeshShape() } } -void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) +void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) { - struct MyNodeOverlapCallback : public btNodeOverlapCallback + struct MyNodeOverlapCallback : public btNodeOverlapCallback { - btStridingMeshInterface* m_meshInterface; + btStridingMeshInterface* m_meshInterface; btTriangleCallback* m_callback; - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(callback) + MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface) + : m_meshInterface(meshInterface), + m_callback(callback) { } - + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { btVector3 m_triangle[3]; - const unsigned char *vertexbase; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; int numfaces; PHY_ScalarType indicestype; @@ -126,60 +123,65 @@ void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const indicestype, nodeSubPart); - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); - + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } + if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); + + m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ()); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + + m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ()); } } /* Perform ray vs. triangle collision here */ - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex); m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } }; - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface); - m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget); + m_bvh->reportRayOverlappingNodex(&myNodeCallback, raySource, rayTarget); } -void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) +void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) { - struct MyNodeOverlapCallback : public btNodeOverlapCallback + struct MyNodeOverlapCallback : public btNodeOverlapCallback { - btStridingMeshInterface* m_meshInterface; + btStridingMeshInterface* m_meshInterface; btTriangleCallback* m_callback; - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(callback) + MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface) + : m_meshInterface(meshInterface), + m_callback(callback) { } - + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { btVector3 m_triangle[3]; - const unsigned char *vertexbase; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; int numfaces; PHY_ScalarType indicestype; @@ -195,75 +197,79 @@ void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, co indicestype, nodeSubPart); - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); - + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + m_triangle[j] = btVector3(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ()); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + + m_triangle[j] = btVector3(btScalar(graphicsbase[0]) * meshScaling.getX(), btScalar(graphicsbase[1]) * meshScaling.getY(), btScalar(graphicsbase[2]) * meshScaling.getZ()); } } /* Perform ray vs. triangle collision here */ - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex); m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } }; - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface); - m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); + m_bvh->reportBoxCastOverlappingNodex(&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); } //perform bvh tree traversal and report overlapping triangles to 'callback' -void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - #ifdef DISABLE_BVH //brute force traverse all triangles - btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax); + btTriangleMeshShape::processAllTriangles(callback, aabbMin, aabbMax); #else //first get all the nodes - - struct MyNodeOverlapCallback : public btNodeOverlapCallback + struct MyNodeOverlapCallback : public btNodeOverlapCallback { - btStridingMeshInterface* m_meshInterface; - btTriangleCallback* m_callback; - btVector3 m_triangle[3]; - + btStridingMeshInterface* m_meshInterface; + btTriangleCallback* m_callback; + btVector3 m_triangle[3]; + int m_numOverlap; - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(callback) + MyNodeOverlapCallback(btTriangleCallback* callback, btStridingMeshInterface* meshInterface) + : m_meshInterface(meshInterface), + m_callback(callback), + m_numOverlap(0) { } - + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { - const unsigned char *vertexbase; + m_numOverlap++; + const unsigned char* vertexbase; int numverts; PHY_ScalarType type; int stride; - const unsigned char *indexbase; + const unsigned char* indexbase; int indexstride; int numfaces; PHY_ScalarType indicestype; - m_meshInterface->getLockedReadOnlyVertexIndexBase( &vertexbase, @@ -276,68 +282,62 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co indicestype, nodeSubPart); - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT||indicestype==PHY_UCHAR); - + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT || indicestype == PHY_UCHAR); + const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:indicestype==PHY_INTEGER?gfxbase[j]:((unsigned char*)gfxbase)[j]; - + int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : indicestype == PHY_INTEGER ? gfxbase[j] : ((unsigned char*)gfxbase)[j]; #ifdef DEBUG_TRIANGLE_MESH - printf("%d ,",graphicsindex); -#endif //DEBUG_TRIANGLE_MESH + printf("%d ,", graphicsindex); +#endif //DEBUG_TRIANGLE_MESH if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); + m_triangle[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); + graphicsbase[0] * meshScaling.getX(), + graphicsbase[1] * meshScaling.getY(), + graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); m_triangle[j] = btVector3( - btScalar(graphicsbase[0])*meshScaling.getX(), - btScalar(graphicsbase[1])*meshScaling.getY(), - btScalar(graphicsbase[2])*meshScaling.getZ()); + btScalar(graphicsbase[0]) * meshScaling.getX(), + btScalar(graphicsbase[1]) * meshScaling.getY(), + btScalar(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 + 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,nodeSubPart,nodeTriangleIndex); + m_callback->processTriangle(m_triangle, nodeSubPart, nodeTriangleIndex); m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); } - }; - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); - - m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); - - -#endif//DISABLE_BVH + MyNodeOverlapCallback myNodeCallback(callback, m_meshInterface); + m_bvh->reportAabbOverlappingNodex(&myNodeCallback, aabbMin, aabbMax); +#endif //DISABLE_BVH } -void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) { - if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) - { - btTriangleMeshShape::setLocalScaling(scaling); - buildOptimizedBvh(); - } + if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON) + { + btTriangleMeshShape::setLocalScaling(scaling); + buildOptimizedBvh(); + } } -void btBvhTriangleMeshShape::buildOptimizedBvh() +void btBvhTriangleMeshShape::buildOptimizedBvh() { if (m_ownsBvh) { @@ -345,43 +345,39 @@ void btBvhTriangleMeshShape::buildOptimizedBvh() btAlignedFree(m_bvh); } ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work - void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16); - m_bvh = new(mem) btOptimizedBvh(); + void* mem = btAlignedAlloc(sizeof(btOptimizedBvh), 16); + m_bvh = new (mem) btOptimizedBvh(); //rebuild the bvh... - m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax); + m_bvh->build(m_meshInterface, m_useQuantizedAabbCompression, m_localAabbMin, m_localAabbMax); m_ownsBvh = true; } -void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling) +void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling) { - btAssert(!m_bvh); - btAssert(!m_ownsBvh); - - m_bvh = bvh; - m_ownsBvh = false; - // update the scaling without rebuilding the bvh - if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) - { - btTriangleMeshShape::setLocalScaling(scaling); - } -} - + btAssert(!m_bvh); + btAssert(!m_ownsBvh); + m_bvh = bvh; + m_ownsBvh = false; + // update the scaling without rebuilding the bvh + if ((getLocalScaling() - scaling).length2() > SIMD_EPSILON) + { + btTriangleMeshShape::setLocalScaling(scaling); + } +} ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*) dataBuffer; + btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*)dataBuffer; - btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer); + btCollisionShape::serialize(&trimeshData->m_collisionShapeData, serializer); m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer); trimeshData->m_collisionMargin = float(m_collisionMargin); - - - if (m_bvh && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_BVH)) + if (m_bvh && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_BVH)) { void* chunk = serializer->findPointer(m_bvh); if (chunk) @@ -390,77 +386,77 @@ const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* se trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)chunk; trimeshData->m_quantizedFloatBvh = 0; #else - trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk; - trimeshData->m_quantizedDoubleBvh= 0; -#endif //BT_USE_DOUBLE_PRECISION - } else + trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)chunk; + trimeshData->m_quantizedDoubleBvh = 0; +#endif //BT_USE_DOUBLE_PRECISION + } + else { - #ifdef BT_USE_DOUBLE_PRECISION trimeshData->m_quantizedDoubleBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); trimeshData->m_quantizedFloatBvh = 0; #else - trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); - trimeshData->m_quantizedDoubleBvh= 0; -#endif //BT_USE_DOUBLE_PRECISION - + trimeshData->m_quantizedFloatBvh = (btQuantizedBvhData*)serializer->getUniquePointer(m_bvh); + trimeshData->m_quantizedDoubleBvh = 0; +#endif //BT_USE_DOUBLE_PRECISION + int sz = m_bvh->calculateSerializeBufferSizeNew(); - btChunk* chunk = serializer->allocate(sz,1); + btChunk* chunk = serializer->allocate(sz, 1); const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,m_bvh); + serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, m_bvh); } - } else + } + else { trimeshData->m_quantizedFloatBvh = 0; trimeshData->m_quantizedDoubleBvh = 0; } - - - if (m_triangleInfoMap && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_TRIANGLEINFOMAP)) + if (m_triangleInfoMap && !(serializer->getSerializationFlags() & BT_SERIALIZE_NO_TRIANGLEINFOMAP)) { void* chunk = serializer->findPointer(m_triangleInfoMap); if (chunk) { trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)chunk; - } else + } + else { trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)serializer->getUniquePointer(m_triangleInfoMap); int sz = m_triangleInfoMap->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(sz,1); + btChunk* chunk = serializer->allocate(sz, 1); const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,m_triangleInfoMap); + serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, m_triangleInfoMap); } - } else + } + else { trimeshData->m_triangleInfoMap = 0; } + // Fill padding with zeros to appease msan. + memset(trimeshData->m_pad3, 0, sizeof(trimeshData->m_pad3)); + return "btTriangleMeshShapeData"; } -void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const +void btBvhTriangleMeshShape::serializeSingleBvh(btSerializer* serializer) const { if (m_bvh) { - int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place - btChunk* chunk = serializer->allocate(len,1); + int len = m_bvh->calculateSerializeBufferSizeNew(); //make sure not to use calculateSerializeBufferSize because it is used for in-place + btChunk* chunk = serializer->allocate(len, 1); const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,(void*)m_bvh); + serializer->finalizeChunk(chunk, structType, BT_QUANTIZED_BVH_CODE, (void*)m_bvh); } } -void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const +void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const { if (m_triangleInfoMap) { int len = m_triangleInfoMap->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = m_triangleInfoMap->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_TRIANLGE_INFO_MAP,(void*)m_triangleInfoMap); + serializer->finalizeChunk(chunk, structType, BT_TRIANLGE_INFO_MAP, (void*)m_triangleInfoMap); } } - - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 1fa4995d160..8b2f2ee85e8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -23,103 +23,99 @@ subject to the following restrictions: ///The btBvhTriangleMeshShape is a static-triangle mesh shape, it can only be used for fixed/non-moving objects. ///If you required moving concave triangle meshes, it is recommended to perform convex decomposition -///using HACD, see Bullet/Demos/ConvexDecompositionDemo. +///using HACD, see Bullet/Demos/ConvexDecompositionDemo. ///Alternatively, you can use btGimpactMeshShape for moving concave triangle meshes. -///btBvhTriangleMeshShape has several optimizations, such as bounding volume hierarchy and -///cache friendly traversal for PlayStation 3 Cell SPU. +///btBvhTriangleMeshShape has several optimizations, such as bounding volume hierarchy and +///cache friendly traversal for PlayStation 3 Cell SPU. ///It is recommended to enable useQuantizedAabbCompression for better memory usage. ///It takes a triangle mesh as input, for example a btTriangleMesh or btTriangleIndexVertexArray. The btBvhTriangleMeshShape class allows for triangle mesh deformations by a refit or partialRefit method. ///Instead of building the bounding volume hierarchy acceleration structure, it is also possible to serialize (save) and deserialize (load) the structure from disk. ///See Demos\ConcaveDemo\ConcavePhysicsDemo.cpp for an example. -ATTRIBUTE_ALIGNED16(class) btBvhTriangleMeshShape : public btTriangleMeshShape +ATTRIBUTE_ALIGNED16(class) +btBvhTriangleMeshShape : public btTriangleMeshShape { - - btOptimizedBvh* m_bvh; - btTriangleInfoMap* m_triangleInfoMap; + btOptimizedBvh* m_bvh; + btTriangleInfoMap* m_triangleInfoMap; bool m_useQuantizedAabbCompression; bool m_ownsBvh; #ifdef __clang__ - bool m_pad[11] __attribute__((unused));////need padding due to alignment + bool m_pad[11] __attribute__((unused)); ////need padding due to alignment #else - bool m_pad[11];////need padding due to alignment + bool m_pad[11]; ////need padding due to alignment #endif public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true); + btBvhTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true); ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb - btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true); - + btBvhTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax, bool buildBvh = true); + virtual ~btBvhTriangleMeshShape(); - bool getOwnsBvh () const + bool getOwnsBvh() const { return m_ownsBvh; } + void performRaycast(btTriangleCallback * callback, const btVector3& raySource, const btVector3& rayTarget); + void performConvexcast(btTriangleCallback * callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax); - - void performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget); - void performConvexcast (btTriangleCallback* callback, const btVector3& boxSource, const btVector3& boxTarget, const btVector3& boxMin, const btVector3& boxMax); - - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - void refitTree(const btVector3& aabbMin,const btVector3& aabbMax); + void refitTree(const btVector3& aabbMin, const btVector3& aabbMax); ///for a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks - void partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax); + void partialRefitTree(const btVector3& aabbMin, const btVector3& aabbMax); //debugging - virtual const char* getName()const {return "BVHTRIANGLEMESH";} + virtual const char* getName() const { return "BVHTRIANGLEMESH"; } + virtual void setLocalScaling(const btVector3& scaling); - virtual void setLocalScaling(const btVector3& scaling); - - btOptimizedBvh* getOptimizedBvh() + btOptimizedBvh* getOptimizedBvh() { return m_bvh; } - void setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1)); + void setOptimizedBvh(btOptimizedBvh * bvh, const btVector3& localScaling = btVector3(1, 1, 1)); - void buildOptimizedBvh(); + void buildOptimizedBvh(); - bool usesQuantizedAabbCompression() const + bool usesQuantizedAabbCompression() const { - return m_useQuantizedAabbCompression; + return m_useQuantizedAabbCompression; } - void setTriangleInfoMap(btTriangleInfoMap* triangleInfoMap) + void setTriangleInfoMap(btTriangleInfoMap * triangleInfoMap) { m_triangleInfoMap = triangleInfoMap; } - const btTriangleInfoMap* getTriangleInfoMap() const + const btTriangleInfoMap* getTriangleInfoMap() const { return m_triangleInfoMap; } - - btTriangleInfoMap* getTriangleInfoMap() + + btTriangleInfoMap* getTriangleInfoMap() { return m_triangleInfoMap; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - virtual void serializeSingleBvh(btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - virtual void serializeSingleTriangleInfoMap(btSerializer* serializer) const; + virtual void serializeSingleBvh(btSerializer * serializer) const; + virtual void serializeSingleTriangleInfoMap(btSerializer * serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btTriangleMeshShapeData { @@ -138,12 +134,11 @@ struct btTriangleMeshShapeData }; +// clang-format on -SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const { return sizeof(btTriangleMeshShapeData); } - - -#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H +#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index 864df26e931..7c337742841 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -13,24 +13,21 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btCapsuleShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" -btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () +btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape() { + m_collisionMargin = radius; m_shapeType = CAPSULE_SHAPE_PROXYTYPE; m_upAxis = 1; - m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); + m_implicitShapeDimensions.setValue(radius, 0.5f * height, radius); } - - btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - - btVector3 supVec(0,0,0); + btVector3 supVec(0, 0, 0); btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); @@ -38,24 +35,22 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } btVector3 vtx; btScalar newDot; - - btScalar radius = getRadius(); - { - btVector3 pos(0,0,0); + btVector3 pos(0, 0, 0); pos[getUpAxis()] = getHalfHeight(); - vtx = pos +vec*(radius) - vec * getMargin(); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -64,10 +59,10 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter } } { - btVector3 pos(0,0,0); + btVector3 pos(0, 0, 0); pos[getUpAxis()] = -getHalfHeight(); - vtx = pos +vec*(radius) - vec * getMargin(); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -77,16 +72,11 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter } return supVec; - } - void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - - - btScalar radius = getRadius(); - - for (int j=0;j<numVectors;j++) + for (int j = 0; j < numVectors; j++) { btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); const btVector3& vec = vectors[j]; @@ -94,9 +84,9 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter btVector3 vtx; btScalar newDot; { - btVector3 pos(0,0,0); + btVector3 pos(0, 0, 0); pos[getUpAxis()] = getHalfHeight(); - vtx = pos +vec*(radius) - vec * getMargin(); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -105,9 +95,9 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter } } { - btVector3 pos(0,0,0); + btVector3 pos(0, 0, 0); pos[getUpAxis()] = -getHalfHeight(); - vtx = pos +vec*(radius) - vec * getMargin(); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -115,57 +105,44 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter supportVerticesOut[j] = vtx; } } - } } - -void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btCapsuleShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //as an approximation, take the inertia of the box that bounds the spheres btTransform ident; ident.setIdentity(); - btScalar radius = getRadius(); - btVector3 halfExtents(radius,radius,radius); - halfExtents[getUpAxis()]+=getHalfHeight(); - - btScalar margin = CONVEX_DISTANCE_MARGIN; + btVector3 halfExtents(radius, radius, radius); + halfExtents[getUpAxis()] += getHalfHeight(); - btScalar lx=btScalar(2.)*(halfExtents[0]+margin); - btScalar ly=btScalar(2.)*(halfExtents[1]+margin); - btScalar lz=btScalar(2.)*(halfExtents[2]+margin); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = btScalar(2.) * (halfExtents[0]); + btScalar ly = btScalar(2.) * (halfExtents[1]); + btScalar lz = btScalar(2.) * (halfExtents[2]); + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(.08333333); - inertia[0] = scaledmass * (y2+z2); - inertia[1] = scaledmass * (x2+z2); - inertia[2] = scaledmass * (x2+y2); - + inertia[0] = scaledmass * (y2 + z2); + inertia[1] = scaledmass * (x2 + z2); + inertia[2] = scaledmass * (x2 + y2); } -btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) +btCapsuleShapeX::btCapsuleShapeX(btScalar radius, btScalar height) { + m_collisionMargin = radius; m_upAxis = 0; - m_implicitShapeDimensions.setValue(0.5f*height, radius,radius); + m_implicitShapeDimensions.setValue(0.5f * height, radius, radius); } - - - - - -btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height) +btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius, btScalar height) { + m_collisionMargin = radius; m_upAxis = 2; - m_implicitShapeDimensions.setValue(radius,radius,0.5f*height); + m_implicitShapeDimensions.setValue(radius, radius, 0.5f * height); } - - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h index f8c55ace4eb..138d0c0f7c1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -17,108 +17,96 @@ subject to the following restrictions: #define BT_CAPSULE_SHAPE_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types - +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis. ///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. ///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres. -ATTRIBUTE_ALIGNED16(class) btCapsuleShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btCapsuleShape : public btConvexInternalShape { protected: - int m_upAxis; + int m_upAxis; protected: ///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses. - btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;}; + btCapsuleShape() : btConvexInternalShape() { m_shapeType = CAPSULE_SHAPE_PROXYTYPE; }; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btCapsuleShape(btScalar radius,btScalar height); + + btCapsuleShape(btScalar radius, btScalar height); ///CollisionShape Interface - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; /// btConvexShape Interface - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual void setMargin(btScalar collisionMargin) { - //correct the m_implicitShapeDimensions for the margin - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; - - btConvexInternalShape::setMargin(collisionMargin); - btVector3 newMargin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; - + //don't override the margin for capsules, their entire radius == margin + (void)collisionMargin; } - virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 halfExtents(getRadius(),getRadius(),getRadius()); - halfExtents[m_upAxis] = getRadius() + getHalfHeight(); - halfExtents += btVector3(getMargin(),getMargin(),getMargin()); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - - aabbMin = center - extent; - aabbMax = center + extent; + btVector3 halfExtents(getRadius(), getRadius(), getRadius()); + halfExtents[m_upAxis] = getRadius() + getHalfHeight(); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + + aabbMin = center - extent; + aabbMax = center + extent; } - virtual const char* getName()const + virtual const char* getName() const { return "CapsuleShape"; } - int getUpAxis() const + int getUpAxis() const { return m_upAxis; } - btScalar getRadius() const + btScalar getRadius() const { - int radiusAxis = (m_upAxis+2)%3; + int radiusAxis = (m_upAxis + 2) % 3; return m_implicitShapeDimensions[radiusAxis]; } - btScalar getHalfHeight() const + btScalar getHalfHeight() const { return m_implicitShapeDimensions[m_upAxis]; } - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; - btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; - + btVector3 unScaledImplicitShapeDimensions = m_implicitShapeDimensions / m_localScaling; btConvexInternalShape::setLocalScaling(scaling); - - m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; - + m_implicitShapeDimensions = (unScaledImplicitShapeDimensions * scaling); + //update m_collisionMargin, since entire radius==margin + int radiusAxis = (m_upAxis + 2) % 3; + m_collisionMargin = m_implicitShapeDimensions[radiusAxis]; } - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - btVector3 aniDir(0,0,0); - aniDir[getUpAxis()]=1; + btVector3 aniDir(0, 0, 0); + aniDir[getUpAxis()] = 1; return aniDir; } - - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - SIMD_FORCE_INLINE void deSerializeFloat(struct btCapsuleShapeData* dataBuffer); + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + SIMD_FORCE_INLINE void deSerializeFloat(struct btCapsuleShapeData * dataBuffer); }; ///btCapsuleShapeX represents a capsule around the Z axis @@ -126,17 +114,13 @@ public: class btCapsuleShapeX : public btCapsuleShape { public: + btCapsuleShapeX(btScalar radius, btScalar height); - btCapsuleShapeX(btScalar radius,btScalar height); - //debugging - virtual const char* getName()const + virtual const char* getName() const { return "CapsuleX"; } - - - }; ///btCapsuleShapeZ represents a capsule around the Z axis @@ -144,45 +128,49 @@ public: class btCapsuleShapeZ : public btCapsuleShape { public: - btCapsuleShapeZ(btScalar radius,btScalar height); + btCapsuleShapeZ(btScalar radius, btScalar height); - //debugging - virtual const char* getName()const + //debugging + virtual const char* getName() const { return "CapsuleZ"; } - - }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btCapsuleShapeData +struct btCapsuleShapeData { - btConvexInternalShapeData m_convexInternalShapeData; + btConvexInternalShapeData m_convexInternalShapeData; - int m_upAxis; + int m_upAxis; - char m_padding[4]; + char m_padding[4]; }; -SIMD_FORCE_INLINE int btCapsuleShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btCapsuleShape::calculateSerializeBufferSize() const { return sizeof(btCapsuleShapeData); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer; - - btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); + btCapsuleShapeData* shapeData = (btCapsuleShapeData*)dataBuffer; + + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); shapeData->m_upAxis = m_upAxis; - + + // Fill padding with zeros to appease msan. + shapeData->m_padding[0] = 0; + shapeData->m_padding[1] = 0; + shapeData->m_padding[2] = 0; + shapeData->m_padding[3] = 0; + return "btCapsuleShapeData"; } -SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer) +SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* dataBuffer) { m_implicitShapeDimensions.deSerializeFloat(dataBuffer->m_convexInternalShapeData.m_implicitShapeDimensions); m_collisionMargin = dataBuffer->m_convexInternalShapeData.m_collisionMargin; @@ -191,4 +179,4 @@ SIMD_FORCE_INLINE void btCapsuleShape::deSerializeFloat(btCapsuleShapeData* data m_upAxis = dataBuffer->m_upAxis; } -#endif //BT_CAPSULE_SHAPE_H +#endif //BT_CAPSULE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h index 474bf1fb499..abd8ab3eb5f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h @@ -19,9 +19,6 @@ subject to the following restrictions: ///The CONVEX_DISTANCE_MARGIN is a default collision margin for convex collision shapes derived from btConvexInternalShape. ///This collision margin is used by Gjk and some other algorithms ///Note that when creating small objects, you need to make sure to set a smaller collision margin, using the 'setMargin' API -#define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01) - - - -#endif //BT_COLLISION_MARGIN_H +#define CONVEX_DISTANCE_MARGIN btScalar(0.04) // btScalar(0.1)//;//btScalar(0.01) +#endif //BT_COLLISION_MARGIN_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp index 39ee21cad73..0b3640a65be 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -20,47 +20,44 @@ subject to the following restrictions: can be used by probes that are checking whether the library is actually installed. */ -extern "C" +extern "C" { -void btBulletCollisionProbe (); + void btBulletCollisionProbe(); -void btBulletCollisionProbe () {} + void btBulletCollisionProbe() {} } - - -void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const +void btCollisionShape::getBoundingSphere(btVector3& center, btScalar& radius) const { btTransform tr; tr.setIdentity(); - btVector3 aabbMin,aabbMax; + btVector3 aabbMin, aabbMax; - getAabb(tr,aabbMin,aabbMax); + getAabb(tr, aabbMin, aabbMax); - radius = (aabbMax-aabbMin).length()*btScalar(0.5); - center = (aabbMin+aabbMax)*btScalar(0.5); + radius = (aabbMax - aabbMin).length() * btScalar(0.5); + center = (aabbMin + aabbMax) * btScalar(0.5); } - -btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const +btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const { return getAngularMotionDisc() * defaultContactThreshold; } -btScalar btCollisionShape::getAngularMotionDisc() const +btScalar btCollisionShape::getAngularMotionDisc() const { ///@todo cache this value, to improve performance - btVector3 center; + btVector3 center; btScalar disc; - getBoundingSphere(center,disc); + getBoundingSphere(center, disc); disc += (center).length(); return disc; } -void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const +void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btVector3& temporalAabbMin, btVector3& temporalAabbMax) const { //start with static aabb - getAabb(curTrans,temporalAabbMin,temporalAabbMax); + getAabb(curTrans, temporalAabbMin, temporalAabbMax); btScalar temporalAabbMaxx = temporalAabbMax.getX(); btScalar temporalAabbMaxy = temporalAabbMax.getY(); @@ -70,50 +67,53 @@ void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const b btScalar temporalAabbMinz = temporalAabbMin.getZ(); // add linear motion - btVector3 linMotion = linvel*timeStep; + btVector3 linMotion = linvel * timeStep; ///@todo: simd would have a vector max/min operation, instead of per-element access if (linMotion.x() > btScalar(0.)) - temporalAabbMaxx += linMotion.x(); + temporalAabbMaxx += linMotion.x(); else temporalAabbMinx += linMotion.x(); if (linMotion.y() > btScalar(0.)) - temporalAabbMaxy += linMotion.y(); + temporalAabbMaxy += linMotion.y(); else temporalAabbMiny += linMotion.y(); if (linMotion.z() > btScalar(0.)) - temporalAabbMaxz += linMotion.z(); + temporalAabbMaxz += linMotion.z(); else temporalAabbMinz += linMotion.z(); //add conservative angular motion btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep; - btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); - temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); - temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); + btVector3 angularMotion3d(angularMotion, angularMotion, angularMotion); + temporalAabbMin = btVector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); + temporalAabbMax = btVector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); temporalAabbMin -= angularMotion3d; temporalAabbMax += angularMotion3d; } ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btCollisionShapeData* shapeData = (btCollisionShapeData*) dataBuffer; - char* name = (char*) serializer->findNameForPointer(this); + btCollisionShapeData* shapeData = (btCollisionShapeData*)dataBuffer; + char* name = (char*)serializer->findNameForPointer(this); shapeData->m_name = (char*)serializer->getUniquePointer(name); if (shapeData->m_name) { serializer->serializeName(name); } shapeData->m_shapeType = m_shapeType; - //shapeData->m_padding//?? + + // Fill padding with zeros to appease msan. + memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding)); + return "btCollisionShapeData"; } -void btCollisionShape::serializeSingleShape(btSerializer* serializer) const +void btCollisionShape::serializeSingleShape(btSerializer* serializer) const { int len = calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,(void*)this); + serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, (void*)this); }
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index 5e86568005e..c80e105a4d8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -19,12 +19,12 @@ subject to the following restrictions: #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types class btSerializer; - ///The btCollisionShape class provides an interface for collision shapes that can be shared among btCollisionObjects. -ATTRIBUTE_ALIGNED16(class) btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btCollisionShape { protected: int m_shapeType; @@ -32,10 +32,9 @@ protected: int m_userIndex; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1) + btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1) { } @@ -44,50 +43,47 @@ public: } ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0; - virtual void getBoundingSphere(btVector3& center,btScalar& radius) const; + virtual void getBoundingSphere(btVector3 & center, btScalar & radius) const; - ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. - virtual btScalar getAngularMotionDisc() const; - - virtual btScalar getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const; + ///getAngularMotionDisc returns the maximum radius needed for Conservative Advancement to handle time-of-impact with rotations. + virtual btScalar getAngularMotionDisc() const; + virtual btScalar getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const; ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative - void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const; - + void calculateTemporalAabb(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btVector3& temporalAabbMin, btVector3& temporalAabbMax) const; - - SIMD_FORCE_INLINE bool isPolyhedral() const + SIMD_FORCE_INLINE bool isPolyhedral() const { return btBroadphaseProxy::isPolyhedral(getShapeType()); } - SIMD_FORCE_INLINE bool isConvex2d() const + SIMD_FORCE_INLINE bool isConvex2d() const { return btBroadphaseProxy::isConvex2d(getShapeType()); } - SIMD_FORCE_INLINE bool isConvex() const + SIMD_FORCE_INLINE bool isConvex() const { return btBroadphaseProxy::isConvex(getShapeType()); } - SIMD_FORCE_INLINE bool isNonMoving() const + SIMD_FORCE_INLINE bool isNonMoving() const { return btBroadphaseProxy::isNonMoving(getShapeType()); } - SIMD_FORCE_INLINE bool isConcave() const + SIMD_FORCE_INLINE bool isConcave() const { return btBroadphaseProxy::isConcave(getShapeType()); } - SIMD_FORCE_INLINE bool isCompound() const + SIMD_FORCE_INLINE bool isCompound() const { return btBroadphaseProxy::isCompound(getShapeType()); } - SIMD_FORCE_INLINE bool isSoftBody() const + SIMD_FORCE_INLINE bool isSoftBody() const { return btBroadphaseProxy::isSoftBody(getShapeType()); } @@ -99,35 +95,35 @@ public: } #ifndef __SPU__ - virtual void setLocalScaling(const btVector3& scaling) =0; - virtual const btVector3& getLocalScaling() const =0; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const = 0; - + virtual void setLocalScaling(const btVector3& scaling) = 0; + virtual const btVector3& getLocalScaling() const = 0; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const = 0; -//debugging support - virtual const char* getName()const =0 ; -#endif //__SPU__ + //debugging support + virtual const char* getName() const = 0; +#endif //__SPU__ - - int getShapeType() const { return m_shapeType; } + int getShapeType() const + { + return m_shapeType; + } ///the getAnisotropicRollingFrictionDirection can be used in combination with setAnisotropicFriction ///See Bullet/Demos/RollingFrictionDemo for an example - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - return btVector3(1,1,1); + return btVector3(1, 1, 1); } - virtual void setMargin(btScalar margin) = 0; - virtual btScalar getMargin() const = 0; + virtual void setMargin(btScalar margin) = 0; + virtual btScalar getMargin() const = 0; - ///optional user data pointer - void setUserPointer(void* userPtr) + void setUserPointer(void* userPtr) { m_userPointer = userPtr; } - void* getUserPointer() const + void* getUserPointer() const { return m_userPointer; } @@ -141,16 +137,16 @@ public: return m_userIndex; } - - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - virtual void serializeSingleShape(btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; -}; + virtual void serializeSingleShape(btSerializer * serializer) const; +}; +// clang-format off +// parser needs * with the name ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btCollisionShapeData { @@ -158,13 +154,10 @@ struct btCollisionShapeData int m_shapeType; char m_padding[4]; }; - -SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const +// clang-format on +SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const { return sizeof(btCollisionShapeData); } - - -#endif //BT_COLLISION_SHAPE_H - +#endif //BT_COLLISION_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index 1e7f36e0a17..fd7828b1047 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -19,26 +19,25 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" btCompoundShape::btCompoundShape(bool enableDynamicAabbTree, const int initialChildCapacity) -: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)), -m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)), -m_dynamicAabbTree(0), -m_updateRevision(1), -m_collisionMargin(btScalar(0.)), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) + : m_localAabbMin(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)), + m_localAabbMax(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)), + m_dynamicAabbTree(0), + m_updateRevision(1), + m_collisionMargin(btScalar(0.)), + m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)) { m_shapeType = COMPOUND_SHAPE_PROXYTYPE; if (enableDynamicAabbTree) { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); + void* mem = btAlignedAlloc(sizeof(btDbvt), 16); + m_dynamicAabbTree = new (mem) btDbvt(); + btAssert(mem == m_dynamicAabbTree); } m_children.reserve(initialChildCapacity); } - btCompoundShape::~btCompoundShape() { if (m_dynamicAabbTree) @@ -48,7 +47,7 @@ btCompoundShape::~btCompoundShape() } } -void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) +void btCompoundShape::addChildShape(const btTransform& localTransform, btCollisionShape* shape) { m_updateRevision++; //m_childTransforms.push_back(localTransform); @@ -60,11 +59,10 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio child.m_childShapeType = shape->getShapeType(); child.m_childMargin = shape->getMargin(); - //extend the local aabbMin/aabbMax - btVector3 localAabbMin,localAabbMax; - shape->getAabb(localTransform,localAabbMin,localAabbMax); - for (int i=0;i<3;i++) + btVector3 localAabbMin, localAabbMax; + shape->getAabb(localTransform, localAabbMin, localAabbMax); + for (int i = 0; i < 3; i++) { if (m_localAabbMin[i] > localAabbMin[i]) { @@ -74,31 +72,30 @@ void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisio { m_localAabbMax[i] = localAabbMax[i]; } - } if (m_dynamicAabbTree) { - const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); size_t index = m_children.size(); - child.m_node = m_dynamicAabbTree->insert(bounds,reinterpret_cast<void*>(index) ); + child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index)); } m_children.push_back(child); - } -void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,bool shouldRecalculateLocalAabb) +void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb) { m_children[childIndex].m_transform = newChildTransform; if (m_dynamicAabbTree) { ///update the dynamic aabb tree - btVector3 localAabbMin,localAabbMax; - m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax); - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + btVector3 localAabbMin, localAabbMax; + m_children[childIndex].m_childShape->getAabb(newChildTransform, localAabbMin, localAabbMax); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); //int index = m_children.size()-1; - m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds); + m_dynamicAabbTree->update(m_children[childIndex].m_node, bounds); } if (shouldRecalculateLocalAabb) @@ -110,35 +107,30 @@ void btCompoundShape::updateChildTransform(int childIndex, const btTransform& ne void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) { m_updateRevision++; - btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size()); + btAssert(childShapeIndex >= 0 && childShapeIndex < m_children.size()); if (m_dynamicAabbTree) { m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node); } - m_children.swap(childShapeIndex,m_children.size()-1); - if (m_dynamicAabbTree) + m_children.swap(childShapeIndex, m_children.size() - 1); + if (m_dynamicAabbTree) m_children[childShapeIndex].m_node->dataAsInt = childShapeIndex; m_children.pop_back(); - } - - void btCompoundShape::removeChildShape(btCollisionShape* shape) { m_updateRevision++; // Find the children containing the shape specified, and remove those children. //note: there might be multiple children using the same shape! - for(int i = m_children.size()-1; i >= 0 ; i--) + for (int i = m_children.size() - 1; i >= 0; i--) { - if(m_children[i].m_childShape == shape) + if (m_children[i].m_childShape == shape) { removeChildShapeByIndex(i); } } - - recalculateLocalAabb(); } @@ -147,15 +139,15 @@ void btCompoundShape::recalculateLocalAabb() // Recalculate the local aabb // Brute force, it iterates over all the shapes left. - m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); //extend the local aabbMin/aabbMax for (int j = 0; j < m_children.size(); j++) { - btVector3 localAabbMin,localAabbMax; + btVector3 localAabbMin, localAabbMax; m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { if (m_localAabbMin[i] > localAabbMin[i]) m_localAabbMin[i] = localAabbMin[i]; @@ -166,54 +158,48 @@ void btCompoundShape::recalculateLocalAabb() } ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btCompoundShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); - btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); - + btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin); + btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin); + //avoid an illegal AABB when there are no children if (!m_children.size()) { - localHalfExtents.setValue(0,0,0); - localCenter.setValue(0,0,0); + localHalfExtents.setValue(0, 0, 0); + localCenter.setValue(0, 0, 0); } - localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); - + localHalfExtents += btVector3(getMargin(), getMargin(), getMargin()); btMatrix3x3 abs_b = trans.getBasis().absolute(); btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - aabbMin = center-extent; - aabbMax = center+extent; - + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + aabbMin = center - extent; + aabbMax = center + extent; } -void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btCompoundShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //approximation: take the inertia from the aabb for now btTransform ident; ident.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(ident,aabbMin,aabbMax); + btVector3 aabbMin, aabbMax; + getAabb(ident, aabbMin, aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz); - inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz); - inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia[0] = mass / (btScalar(12.0)) * (ly * ly + lz * lz); + inertia[1] = mass / (btScalar(12.0)) * (lx * lx + lz * lz); + inertia[2] = mass / (btScalar(12.0)) * (lx * lx + ly * ly); } - - - -void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const +void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const { int n = m_children.size(); @@ -223,18 +209,18 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf for (k = 0; k < n; k++) { - btAssert(masses[k]>0); + btAssert(masses[k] > 0); center += m_children[k].m_transform.getOrigin() * masses[k]; totalMass += masses[k]; } - btAssert(totalMass>0); + btAssert(totalMass > 0); center /= totalMass; principal.setOrigin(center); btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0); - for ( k = 0; k < n; k++) + for (k = 0; k < n; k++) { btVector3 i; m_children[k].m_childShape->calculateLocalInertia(masses[k], i); @@ -259,8 +245,8 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf j[0].setValue(o2, 0, 0); j[1].setValue(0, o2, 0); j[2].setValue(0, 0, o2); - j[0] += o * -o.x(); - j[1] += o * -o.y(); + j[0] += o * -o.x(); + j[1] += o * -o.y(); j[2] += o * -o.z(); //add inertia tensor of pointmass @@ -273,59 +259,50 @@ void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransf inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]); } - - - - void btCompoundShape::setLocalScaling(const btVector3& scaling) { - - for(int i = 0; i < m_children.size(); i++) + for (int i = 0; i < m_children.size(); i++) { btTransform childTrans = getChildTransform(i); btVector3 childScale = m_children[i].m_childShape->getLocalScaling(); -// childScale = childScale * (childTrans.getBasis() * scaling); + // childScale = childScale * (childTrans.getBasis() * scaling); childScale = childScale * scaling / m_localScaling; m_children[i].m_childShape->setLocalScaling(childScale); childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling); - updateChildTransform(i, childTrans,false); + updateChildTransform(i, childTrans, false); } - + m_localScaling = scaling; recalculateLocalAabb(); - } - void btCompoundShape::createAabbTreeFromChildren() { - if ( !m_dynamicAabbTree ) - { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); + if (!m_dynamicAabbTree) + { + void* mem = btAlignedAlloc(sizeof(btDbvt), 16); + m_dynamicAabbTree = new (mem) btDbvt(); + btAssert(mem == m_dynamicAabbTree); - for ( int index = 0; index < m_children.size(); index++ ) - { - btCompoundShapeChild &child = m_children[index]; + for (int index = 0; index < m_children.size(); index++) + { + btCompoundShapeChild& child = m_children[index]; - //extend the local aabbMin/aabbMax - btVector3 localAabbMin,localAabbMax; - child.m_childShape->getAabb(child.m_transform,localAabbMin,localAabbMax); + //extend the local aabbMin/aabbMax + btVector3 localAabbMin, localAabbMax; + child.m_childShape->getAabb(child.m_transform, localAabbMin, localAabbMax); - const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + const btDbvtVolume bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); size_t index2 = index; - child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2) ); - } - } + child.m_node = m_dynamicAabbTree->insert(bounds, reinterpret_cast<void*>(index2)); + } + } } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const { - - btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer; + btCompoundShapeData* shapeData = (btCompoundShapeData*)dataBuffer; btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); shapeData->m_collisionMargin = float(m_collisionMargin); @@ -333,27 +310,26 @@ const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serialize shapeData->m_childShapePtr = 0; if (shapeData->m_numChildShapes) { - btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData),shapeData->m_numChildShapes); + btChunk* chunk = serializer->allocate(sizeof(btCompoundShapeChildData), shapeData->m_numChildShapes); btCompoundShapeChildData* memPtr = (btCompoundShapeChildData*)chunk->m_oldPtr; shapeData->m_childShapePtr = (btCompoundShapeChildData*)serializer->getUniquePointer(memPtr); - for (int i=0;i<shapeData->m_numChildShapes;i++,memPtr++) + for (int i = 0; i < shapeData->m_numChildShapes; i++, memPtr++) { memPtr->m_childMargin = float(m_children[i].m_childMargin); memPtr->m_childShape = (btCollisionShapeData*)serializer->getUniquePointer(m_children[i].m_childShape); //don't serialize shapes that already have been serialized if (!serializer->findPointer(m_children[i].m_childShape)) { - btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(),1); - const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr,serializer); - serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,m_children[i].m_childShape); - } + btChunk* chunk = serializer->allocate(m_children[i].m_childShape->calculateSerializeBufferSize(), 1); + const char* structType = m_children[i].m_childShape->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_SHAPE_CODE, m_children[i].m_childShape); + } memPtr->m_childShapeType = m_children[i].m_childShapeType; m_children[i].m_transform.serializeFloat(memPtr->m_transform); } - serializer->finalizeChunk(chunk,"btCompoundShapeChildData",BT_ARRAY_CODE,chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btCompoundShapeChildData", BT_ARRAY_CODE, chunk->m_oldPtr); } return "btCompoundShapeData"; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 4eef8dba306..7e2d0eb8170 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -27,45 +27,47 @@ subject to the following restrictions: //class btOptimizedBvh; struct btDbvt; -ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild +ATTRIBUTE_ALIGNED16(struct) +btCompoundShapeChild { BT_DECLARE_ALIGNED_ALLOCATOR(); - btTransform m_transform; - btCollisionShape* m_childShape; - int m_childShapeType; - btScalar m_childMargin; - struct btDbvtNode* m_node; + btTransform m_transform; + btCollisionShape* m_childShape; + int m_childShapeType; + btScalar m_childMargin; + struct btDbvtNode* m_node; }; SIMD_FORCE_INLINE bool operator==(const btCompoundShapeChild& c1, const btCompoundShapeChild& c2) { - return ( c1.m_transform == c2.m_transform && - c1.m_childShape == c2.m_childShape && - c1.m_childShapeType == c2.m_childShapeType && - c1.m_childMargin == c2.m_childMargin ); + return (c1.m_transform == c2.m_transform && + c1.m_childShape == c2.m_childShape && + c1.m_childShapeType == c2.m_childShapeType && + c1.m_childMargin == c2.m_childMargin); } /// The btCompoundShape allows to store multiple other btCollisionShapes /// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape. -/// It has an (optional) dynamic aabb tree to accelerate early rejection tests. +/// It has an (optional) dynamic aabb tree to accelerate early rejection tests. /// @todo: This aabb tree can also be use to speed up ray tests on btCompoundShape, see http://code.google.com/p/bullet/issues/detail?id=25 /// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape) -ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btCompoundShape : public btCollisionShape { protected: btAlignedObjectArray<btCompoundShapeChild> m_children; - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; - btDbvt* m_dynamicAabbTree; + btDbvt* m_dynamicAabbTree; ///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated - int m_updateRevision; + int m_updateRevision; - btScalar m_collisionMargin; + btScalar m_collisionMargin; - btVector3 m_localScaling; + btVector3 m_localScaling; public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -74,17 +76,16 @@ public: virtual ~btCompoundShape(); - void addChildShape(const btTransform& localTransform,btCollisionShape* shape); + void addChildShape(const btTransform& localTransform, btCollisionShape* shape); /// Remove all children shapes that contain the specified shape - virtual void removeChildShape(btCollisionShape* shape); + virtual void removeChildShape(btCollisionShape * shape); void removeChildShapeByIndex(int childShapeindex); - - int getNumChildShapes() const + int getNumChildShapes() const { - return int (m_children.size()); + return int(m_children.size()); } btCollisionShape* getChildShape(int index) @@ -96,18 +97,17 @@ public: return m_children[index].m_childShape; } - btTransform& getChildTransform(int index) + btTransform& getChildTransform(int index) { return m_children[index].m_transform; } - const btTransform& getChildTransform(int index) const + const btTransform& getChildTransform(int index) const { return m_children[index].m_transform; } ///set a new transform for a child, and update internal data structures (local aabb and dynamic tree) - void updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true); - + void updateChildTransform(int childIndex, const btTransform& newChildTransform, bool shouldRecalculateLocalAabb = true); btCompoundShapeChild* getChildList() { @@ -115,40 +115,40 @@ public: } ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; /** Re-calculate the local Aabb. Is called at the end of removeChildShapes. Use this yourself if you modify the children or their transforms. */ - virtual void recalculateLocalAabb(); + virtual void recalculateLocalAabb(); - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const + virtual const btVector3& getLocalScaling() const { return m_localScaling; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { m_collisionMargin = margin; } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { return m_collisionMargin; } - virtual const char* getName()const + virtual const char* getName() const { return "Compound"; } - const btDbvt* getDynamicAabbTree() const + const btDbvt* getDynamicAabbTree() const { return m_dynamicAabbTree; } - - btDbvt* getDynamicAabbTree() + + btDbvt* getDynamicAabbTree() { return m_dynamicAabbTree; } @@ -160,21 +160,21 @@ public: ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform ///of the collision object by the principal transform. - void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const; + void calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const; - int getUpdateRevision() const + int getUpdateRevision() const { return m_updateRevision; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btCompoundShapeChildData { @@ -197,16 +197,11 @@ struct btCompoundShapeData }; +// clang-format on -SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const { return sizeof(btCompoundShapeData); } - - - - - - -#endif //BT_COMPOUND_SHAPE_H +#endif //BT_COMPOUND_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp index 58ff84a5b02..5d396844dd8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp @@ -13,15 +13,12 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btConcaveShape.h" btConcaveShape::btConcaveShape() : m_collisionMargin(btScalar(0.)) { - } btConcaveShape::~btConcaveShape() { - } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h index 2917cc5b60f..716624e182b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -17,12 +17,13 @@ subject to the following restrictions: #define BT_CONCAVE_SHAPE_H #include "btCollisionShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "btTriangleCallback.h" /// PHY_ScalarType enumerates possible scalar types. /// See the btStridingMeshInterface or btHeightfieldTerrainShape for its use -typedef enum PHY_ScalarType { +typedef enum PHY_ScalarType +{ PHY_FLOAT, PHY_DOUBLE, PHY_INTEGER, @@ -33,30 +34,29 @@ typedef enum PHY_ScalarType { ///The btConcaveShape class provides an interface for non-moving (static) concave shapes. ///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape. -ATTRIBUTE_ALIGNED16(class) btConcaveShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btConcaveShape : public btCollisionShape { protected: btScalar m_collisionMargin; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btConcaveShape(); virtual ~btConcaveShape(); - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const = 0; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const = 0; - virtual btScalar getMargin() const { + virtual btScalar getMargin() const + { return m_collisionMargin; } virtual void setMargin(btScalar collisionMargin) { m_collisionMargin = collisionMargin; } - - - }; -#endif //BT_CONCAVE_SHAPE_H +#endif //BT_CONCAVE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp index 2d83c8bfbac..64a6f272ca5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -15,11 +15,9 @@ subject to the following restrictions: #include "btConeShape.h" - - -btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (), -m_radius (radius), -m_height(height) +btConeShape::btConeShape(btScalar radius, btScalar height) : btConvexInternalShape(), + m_radius(radius), + m_height(height) { m_shapeType = CONE_SHAPE_PROXYTYPE; setConeUpIndex(1); @@ -27,42 +25,40 @@ m_height(height) m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height)); } -btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height): -btConeShape(radius,height) +btConeShapeZ::btConeShapeZ(btScalar radius, btScalar height) : btConeShape(radius, height) { setConeUpIndex(2); } -btConeShapeX::btConeShapeX (btScalar radius,btScalar height): -btConeShape(radius,height) +btConeShapeX::btConeShapeX(btScalar radius, btScalar height) : btConeShape(radius, height) { setConeUpIndex(0); } ///choose upAxis index -void btConeShape::setConeUpIndex(int upIndex) +void btConeShape::setConeUpIndex(int upIndex) { switch (upIndex) { - case 0: + case 0: m_coneIndices[0] = 1; m_coneIndices[1] = 0; m_coneIndices[2] = 2; - break; - case 1: + break; + case 1: m_coneIndices[0] = 0; m_coneIndices[1] = 1; m_coneIndices[2] = 2; - break; - case 2: + break; + case 2: m_coneIndices[0] = 0; m_coneIndices[1] = 2; m_coneIndices[2] = 1; - break; - default: - btAssert(0); + break; + default: + btAssert(0); }; - + m_implicitShapeDimensions[m_coneIndices[0]] = m_radius; m_implicitShapeDimensions[m_coneIndices[1]] = m_height; m_implicitShapeDimensions[m_coneIndices[2]] = m_radius; @@ -70,72 +66,71 @@ void btConeShape::setConeUpIndex(int upIndex) btVector3 btConeShape::coneLocalSupport(const btVector3& v) const { - btScalar halfHeight = m_height * btScalar(0.5); - if (v[m_coneIndices[1]] > v.length() * m_sinAngle) - { - btVector3 tmp; - - tmp[m_coneIndices[0]] = btScalar(0.); - tmp[m_coneIndices[1]] = halfHeight; - tmp[m_coneIndices[2]] = btScalar(0.); - return tmp; - } - else { - btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]); - if (s > SIMD_EPSILON) { - btScalar d = m_radius / s; - btVector3 tmp; - tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d; - tmp[m_coneIndices[1]] = -halfHeight; - tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d; - return tmp; - } - else { + if (v[m_coneIndices[1]] > v.length() * m_sinAngle) + { btVector3 tmp; + tmp[m_coneIndices[0]] = btScalar(0.); - tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[1]] = halfHeight; tmp[m_coneIndices[2]] = btScalar(0.); return tmp; } - } - + else + { + btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]); + if (s > SIMD_EPSILON) + { + btScalar d = m_radius / s; + btVector3 tmp; + tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d; + tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d; + return tmp; + } + else + { + btVector3 tmp; + tmp[m_coneIndices[0]] = btScalar(0.); + tmp[m_coneIndices[1]] = -halfHeight; + tmp[m_coneIndices[2]] = btScalar(0.); + return tmp; + } + } } -btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const +btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return coneLocalSupport(vec); + return coneLocalSupport(vec); } -void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { const btVector3& vec = vectors[i]; supportVerticesOut[i] = coneLocalSupport(vec); } } - -btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const +btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = coneLocalSupport(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - -void btConeShape::setLocalScaling(const btVector3& scaling) +void btConeShape::setLocalScaling(const btVector3& scaling) { int axis = m_coneIndices[1]; int r1 = m_coneIndices[0]; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index 4a0df0d558b..ee6786c8074 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -17,155 +17,157 @@ subject to the following restrictions: #define BT_CONE_MINKOWSKI_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///The btConeShape implements a cone shape primitive, centered around the origin and aligned with the Y axis. The btConeShapeX is aligned around the X axis and btConeShapeZ around the Z axis. -ATTRIBUTE_ALIGNED16(class) btConeShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btConeShape : public btConvexInternalShape { - btScalar m_sinAngle; btScalar m_radius; btScalar m_height; - int m_coneIndices[3]; + int m_coneIndices[3]; btVector3 coneLocalSupport(const btVector3& v) const; - public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConeShape (btScalar radius,btScalar height); - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - btScalar getRadius() const { return m_radius;} - btScalar getHeight() const { return m_height;} + btConeShape(btScalar radius, btScalar height); + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const + btScalar getRadius() const { return m_radius; } + btScalar getHeight() const { return m_height; } + + void setRadius(const btScalar radius) + { + m_radius = radius; + } + void setHeight(const btScalar height) + { + m_height = height; + } + + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const { btTransform identity; identity.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(identity,aabbMin,aabbMax); - - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); + btVector3 aabbMin, aabbMax; - btScalar margin = getMargin(); + getAabb(identity, aabbMin, aabbMax); // This already contains the margin + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); - btScalar lx=btScalar(2.)*(halfExtents.x()+margin); - btScalar ly=btScalar(2.)*(halfExtents.y()+margin); - btScalar lz=btScalar(2.)*(halfExtents.z()+margin); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(0.08333333); - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); + + // inertia.x() = scaledmass * (y2+z2); + // inertia.y() = scaledmass * (x2+z2); + // inertia.z() = scaledmass * (x2+y2); + } -// inertia.x() = scaledmass * (y2+z2); -// inertia.y() = scaledmass * (x2+z2); -// inertia.z() = scaledmass * (x2+y2); + virtual const char* getName() const + { + return "Cone"; } + ///choose upAxis index + void setConeUpIndex(int upIndex); - virtual const char* getName()const - { - return "Cone"; - } - - ///choose upAxis index - void setConeUpIndex(int upIndex); - - int getConeUpIndex() const - { - return m_coneIndices[1]; - } - - virtual btVector3 getAnisotropicRollingFrictionDirection() const + int getConeUpIndex() const { - return btVector3 (0,1,0); + return m_coneIndices[1]; } - virtual void setLocalScaling(const btVector3& scaling); - - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual btVector3 getAnisotropicRollingFrictionDirection() const + { + return btVector3(0, 1, 0); + } + + virtual void setLocalScaling(const btVector3& scaling); + + virtual int calculateSerializeBufferSize() const; + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///btConeShape implements a Cone shape, around the X axis class btConeShapeX : public btConeShape { - public: - btConeShapeX(btScalar radius,btScalar height); +public: + btConeShapeX(btScalar radius, btScalar height); - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - return btVector3 (1,0,0); + return btVector3(1, 0, 0); } //debugging - virtual const char* getName()const + virtual const char* getName() const { return "ConeX"; } - - }; ///btConeShapeZ implements a Cone shape, around the Z axis class btConeShapeZ : public btConeShape { public: - btConeShapeZ(btScalar radius,btScalar height); + btConeShapeZ(btScalar radius, btScalar height); - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - return btVector3 (0,0,1); + return btVector3(0, 0, 1); } //debugging - virtual const char* getName()const + virtual const char* getName() const { return "ConeZ"; } - - }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btConeShapeData +struct btConeShapeData { - btConvexInternalShapeData m_convexInternalShapeData; - - int m_upIndex; - - char m_padding[4]; + btConvexInternalShapeData m_convexInternalShapeData; + + int m_upIndex; + + char m_padding[4]; }; -SIMD_FORCE_INLINE int btConeShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btConeShape::calculateSerializeBufferSize() const { return sizeof(btConeShapeData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btConeShapeData* shapeData = (btConeShapeData*) dataBuffer; - - btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); - + btConeShapeData* shapeData = (btConeShapeData*)dataBuffer; + + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); + shapeData->m_upIndex = m_coneIndices[1]; - + + // Fill padding with zeros to appease msan. + shapeData->m_padding[0] = 0; + shapeData->m_padding[1] = 0; + shapeData->m_padding[2] = 0; + shapeData->m_padding[3] = 0; + return "btConeShapeData"; } -#endif //BT_CONE_MINKOWSKI_H - +#endif //BT_CONE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp index 10ea3e981af..7d3d1d362fb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp @@ -15,54 +15,48 @@ subject to the following restrictions: #include "btConvex2dShape.h" -btConvex2dShape::btConvex2dShape( btConvexShape* convexChildShape): -btConvexShape (), m_childConvexShape(convexChildShape) +btConvex2dShape::btConvex2dShape(btConvexShape* convexChildShape) : btConvexShape(), m_childConvexShape(convexChildShape) { m_shapeType = CONVEX_2D_SHAPE_PROXYTYPE; } - + btConvex2dShape::~btConvex2dShape() { } - - -btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { return m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); } -void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); + m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors, supportVerticesOut, numVectors); } - -btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec) const { return m_childConvexShape->localGetSupportingVertex(vec); } - -void btConvex2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btConvex2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { ///this linear upscaling is not realistic, but we don't deal with large mass ratios... - m_childConvexShape->calculateLocalInertia(mass,inertia); + m_childConvexShape->calculateLocalInertia(mass, inertia); } - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btConvex2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btConvex2dShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - m_childConvexShape->getAabb(t,aabbMin,aabbMax); + m_childConvexShape->getAabb(t, aabbMin, aabbMax); } -void btConvex2dShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btConvex2dShape::getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax); + m_childConvexShape->getAabbSlow(t, aabbMin, aabbMax); } -void btConvex2dShape::setLocalScaling(const btVector3& scaling) +void btConvex2dShape::setLocalScaling(const btVector3& scaling) { m_childConvexShape->setLocalScaling(scaling); } @@ -72,21 +66,21 @@ const btVector3& btConvex2dShape::getLocalScaling() const return m_childConvexShape->getLocalScaling(); } -void btConvex2dShape::setMargin(btScalar margin) +void btConvex2dShape::setMargin(btScalar margin) { m_childConvexShape->setMargin(margin); } -btScalar btConvex2dShape::getMargin() const +btScalar btConvex2dShape::getMargin() const { return m_childConvexShape->getMargin(); } -int btConvex2dShape::getNumPreferredPenetrationDirections() const +int btConvex2dShape::getNumPreferredPenetrationDirections() const { return m_childConvexShape->getNumPreferredPenetrationDirections(); } - -void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + +void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { - m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); + m_childConvexShape->getPreferredPenetrationDirection(index, penetrationVector); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h index bbd1caf42fb..cd4f1ef7b89 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h @@ -17,66 +17,61 @@ subject to the following restrictions: #define BT_CONVEX_2D_SHAPE_H #include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///The btConvex2dShape allows to use arbitrary convex shapes as 2d convex shapes, with the Z component assumed to be 0. ///For 2d boxes, the btBox2dShape is recommended. -ATTRIBUTE_ALIGNED16(class) btConvex2dShape : public btConvexShape +ATTRIBUTE_ALIGNED16(class) +btConvex2dShape : public btConvexShape { - btConvexShape* m_childConvexShape; + btConvexShape* m_childConvexShape; - public: - +public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvex2dShape( btConvexShape* convexChildShape); - + + btConvex2dShape(btConvexShape * convexChildShape); + virtual ~btConvex2dShape(); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - btConvexShape* getChildShape() + btConvexShape* getChildShape() { return m_childConvexShape; } - const btConvexShape* getChildShape() const + const btConvexShape* getChildShape() const { return m_childConvexShape; } - virtual const char* getName()const + virtual const char* getName() const { return "Convex2dShape"; } - - /////////////////////////// - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling) ; - virtual const btVector3& getLocalScaling() const ; + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void setMargin(btScalar margin); - virtual btScalar getMargin() const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; - virtual int getNumPreferredPenetrationDirections() const; - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + virtual int getNumPreferredPenetrationDirections() const; + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; }; -#endif //BT_CONVEX_2D_SHAPE_H +#endif //BT_CONVEX_2D_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 02ea5033b15..703de45922e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -13,24 +13,26 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//#if defined (_WIN32) || defined (__i386__) -//#define BT_USE_SSE_IN_API -//#endif +#if defined(_WIN32) || defined(__i386__) +#define BT_USE_SSE_IN_API +#endif #include "btConvexHullShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" #include "LinearMath/btSerializer.h" +#include "btConvexPolyhedron.h" +#include "LinearMath/btConvexHullComputer.h" -btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape () +btConvexHullShape ::btConvexHullShape(const btScalar* points, int numPoints, int stride) : btPolyhedralConvexAabbCachingShape() { m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE; m_unscaledPoints.resize(numPoints); unsigned char* pointsAddress = (unsigned char*)points; - for (int i=0;i<numPoints;i++) + for (int i = 0; i < numPoints; i++) { btScalar* point = (btScalar*)pointsAddress; m_unscaledPoints[i] = btVector3(point[0], point[1], point[2]); @@ -38,11 +40,8 @@ btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int } recalcLocalAabb(); - } - - void btConvexHullShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; @@ -54,83 +53,81 @@ void btConvexHullShape::addPoint(const btVector3& point, bool recalculateLocalAa m_unscaledPoints.push_back(point); if (recalculateLocalAabb) recalcLocalAabb(); - } -btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.)); btScalar maxDot = btScalar(-BT_LARGE_FLOAT); - // Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically. - if( 0 < m_unscaledPoints.size() ) - { - btVector3 scaled = vec * m_localScaling; - int index = (int) scaled.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), maxDot); // FIXME: may violate encapsulation of m_unscaledPoints - return m_unscaledPoints[index] * m_localScaling; - } + // Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically. + if (0 < m_unscaledPoints.size()) + { + btVector3 scaled = vec * m_localScaling; + int index = (int)scaled.maxDot(&m_unscaledPoints[0], m_unscaledPoints.size(), maxDot); // FIXME: may violate encapsulation of m_unscaledPoints + return m_unscaledPoints[index] * m_localScaling; + } - return supVec; + return supVec; } -void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvexHullShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { btScalar newDot; //use 'w' component of supportVerticesOut? { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); } } - for (int j=0;j<numVectors;j++) - { - btVector3 vec = vectors[j] * m_localScaling; // dot(a*b,c) = dot(a,b*c) - if( 0 < m_unscaledPoints.size() ) - { - int i = (int) vec.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), newDot); - supportVerticesOut[j] = getScaledPoint(i); - supportVerticesOut[j][3] = newDot; - } - else - supportVerticesOut[j][3] = -BT_LARGE_FLOAT; - } - - - + for (int j = 0; j < numVectors; j++) + { + btVector3 vec = vectors[j] * m_localScaling; // dot(a*b,c) = dot(a,b*c) + if (0 < m_unscaledPoints.size()) + { + int i = (int)vec.maxDot(&m_unscaledPoints[0], m_unscaledPoints.size(), newDot); + supportVerticesOut[j] = getScaledPoint(i); + supportVerticesOut[j][3] = newDot; + } + else + supportVerticesOut[j][3] = -BT_LARGE_FLOAT; + } } - - -btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - - - - - - - +void btConvexHullShape::optimizeConvexHull() +{ + btConvexHullComputer conv; + conv.compute(&m_unscaledPoints[0].getX(), sizeof(btVector3), m_unscaledPoints.size(), 0.f, 0.f); + int numVerts = conv.vertices.size(); + m_unscaledPoints.resize(0); + for (int i = 0; i < numVerts; i++) + { + m_unscaledPoints.push_back(conv.vertices[i]); + } +} //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo -int btConvexHullShape::getNumVertices() const +int btConvexHullShape::getNumVertices() const { return m_unscaledPoints.size(); } @@ -140,111 +137,108 @@ int btConvexHullShape::getNumEdges() const return m_unscaledPoints.size(); } -void btConvexHullShape::getEdge(int i,btVector3& pa,btVector3& pb) const +void btConvexHullShape::getEdge(int i, btVector3& pa, btVector3& pb) const { - - int index0 = i%m_unscaledPoints.size(); - int index1 = (i+1)%m_unscaledPoints.size(); + int index0 = i % m_unscaledPoints.size(); + int index1 = (i + 1) % m_unscaledPoints.size(); pa = getScaledPoint(index0); pb = getScaledPoint(index1); } -void btConvexHullShape::getVertex(int i,btVector3& vtx) const +void btConvexHullShape::getVertex(int i, btVector3& vtx) const { vtx = getScaledPoint(i); } -int btConvexHullShape::getNumPlanes() const +int btConvexHullShape::getNumPlanes() const { return 0; } -void btConvexHullShape::getPlane(btVector3& ,btVector3& ,int ) const +void btConvexHullShape::getPlane(btVector3&, btVector3&, int) const { - btAssert(0); } //not yet -bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const +bool btConvexHullShape::isInside(const btVector3&, btScalar) const { btAssert(0); return false; } ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* serializer) const { //int szc = sizeof(btConvexHullShapeData); - btConvexHullShapeData* shapeData = (btConvexHullShapeData*) dataBuffer; + btConvexHullShapeData* shapeData = (btConvexHullShapeData*)dataBuffer; btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); int numElem = m_unscaledPoints.size(); shapeData->m_numUnscaledPoints = numElem; #ifdef BT_USE_DOUBLE_PRECISION shapeData->m_unscaledPointsFloatPtr = 0; - shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]): 0; + shapeData->m_unscaledPointsDoublePtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0; #else - shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]): 0; + shapeData->m_unscaledPointsFloatPtr = numElem ? (btVector3Data*)serializer->getUniquePointer((void*)&m_unscaledPoints[0]) : 0; shapeData->m_unscaledPointsDoublePtr = 0; #endif - + if (numElem) { int sz = sizeof(btVector3Data); - // int sz2 = sizeof(btVector3DoubleData); - // int sz3 = sizeof(btVector3FloatData); - btChunk* chunk = serializer->allocate(sz,numElem); + // int sz2 = sizeof(btVector3DoubleData); + // int sz3 = sizeof(btVector3FloatData); + btChunk* chunk = serializer->allocate(sz, numElem); btVector3Data* memPtr = (btVector3Data*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_unscaledPoints[i].serialize(*memPtr); } - serializer->finalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]); + serializer->finalizeChunk(chunk, btVector3DataName, BT_ARRAY_CODE, (void*)&m_unscaledPoints[0]); } - + + // Fill padding with zeros to appease msan. + memset(shapeData->m_padding3, 0, sizeof(shapeData->m_padding3)); + return "btConvexHullShapeData"; } -void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const +void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const { #if 1 minProj = FLT_MAX; maxProj = -FLT_MAX; int numVerts = m_unscaledPoints.size(); - for(int i=0;i<numVerts;i++) + for (int i = 0; i < numVerts; i++) { btVector3 vtx = m_unscaledPoints[i] * m_localScaling; btVector3 pt = trans * vtx; btScalar dp = pt.dot(dir); - if(dp < minProj) + if (dp < minProj) { minProj = dp; witnesPtMin = pt; } - if(dp > maxProj) + if (dp > maxProj) { maxProj = dp; - witnesPtMax=pt; + witnesPtMax = pt; } } #else - btVector3 localAxis = dir*trans.getBasis(); - witnesPtMin = trans(localGetSupportingVertex(localAxis)); + btVector3 localAxis = dir * trans.getBasis(); + witnesPtMin = trans(localGetSupportingVertex(localAxis)); witnesPtMax = trans(localGetSupportingVertex(-localAxis)); minProj = witnesPtMin.dot(dir); maxProj = witnesPtMax.dot(dir); #endif - if(minProj>maxProj) + if (minProj > maxProj) { - btSwap(minProj,maxProj); - btSwap(witnesPtMin,witnesPtMax); + btSwap(minProj, maxProj); + btSwap(witnesPtMin, witnesPtMax); } - - } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h index 3bd598ec4e9..96136d7dd72 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -17,28 +17,26 @@ subject to the following restrictions: #define BT_CONVEX_HULL_SHAPE_H #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btAlignedObjectArray.h" - ///The btConvexHullShape implements an implicit convex hull of an array of vertices. ///Bullet provides a general and fast collision detector for convex shapes based on GJK and EPA using localGetSupportingVertex. -ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btConvexHullShape : public btPolyhedralConvexAabbCachingShape { - btAlignedObjectArray<btVector3> m_unscaledPoints; + btAlignedObjectArray<btVector3> m_unscaledPoints; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive btScalar (x,y,z), the striding defines the number of bytes between each point, in memory. ///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint. ///btConvexHullShape make an internal copy of the points. - btConvexHullShape(const btScalar* points=0,int numPoints=0, int stride=sizeof(btVector3)); + btConvexHullShape(const btScalar* points = 0, int numPoints = 0, int stride = sizeof(btVector3)); void addPoint(const btVector3& point, bool recalculateLocalAabb = true); - btVector3* getUnscaledPoints() { return &m_unscaledPoints[0]; @@ -55,49 +53,46 @@ public: return getUnscaledPoints(); } - - + void optimizeConvexHull(); - SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const + SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const { return m_unscaledPoints[i] * m_localScaling; } - SIMD_FORCE_INLINE int getNumPoints() const + SIMD_FORCE_INLINE int getNumPoints() const { return m_unscaledPoints.size(); } - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - - virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const; //debugging - virtual const char* getName()const {return "Convex";} + virtual const char* getName() const { return "Convex"; } - - virtual int getNumVertices() const; + virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; + virtual void getVertex(int i, btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; ///in case we receive negative scaling - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btConvexHullShapeData { @@ -111,12 +106,11 @@ struct btConvexHullShapeData }; +// clang-format on -SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const { return sizeof(btConvexHullShapeData); } - -#endif //BT_CONVEX_HULL_SHAPE_H - +#endif //BT_CONVEX_HULL_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp index 083d60b1b1e..b847f8f40f6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp @@ -13,139 +13,125 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btConvexInternalShape.h" - - btConvexInternalShape::btConvexInternalShape() -: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), -m_collisionMargin(CONVEX_DISTANCE_MARGIN) + : m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)), + m_collisionMargin(CONVEX_DISTANCE_MARGIN) { } - -void btConvexInternalShape::setLocalScaling(const btVector3& scaling) +void btConvexInternalShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling.absolute(); } - - -void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const +void btConvexInternalShape::getAabbSlow(const btTransform& trans, btVector3& minAabb, btVector3& maxAabb) const { #ifndef __SPU__ //use localGetSupportingVertexWithoutMargin? btScalar margin = getMargin(); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); - btVector3 sv = localGetSupportingVertex(vec*trans.getBasis()); + btVector3 sv = localGetSupportingVertex(vec * trans.getBasis()); btVector3 tmp = trans(sv); - maxAabb[i] = tmp[i]+margin; + maxAabb[i] = tmp[i] + margin; vec[i] = btScalar(-1.); - tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); - minAabb[i] = tmp[i]-margin; + tmp = trans(localGetSupportingVertex(vec * trans.getBasis())); + minAabb[i] = tmp[i] - margin; } #endif } - - -btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec) const { #ifndef __SPU__ - btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; #else btAssert(0); - return btVector3(0,0,0); -#endif //__SPU__ - - } - + return btVector3(0, 0, 0); +#endif //__SPU__ +} btConvexInternalAabbCachingShape::btConvexInternalAabbCachingShape() - : btConvexInternalShape(), -m_localAabbMin(1,1,1), -m_localAabbMax(-1,-1,-1), -m_isLocalAabbValid(false) + : btConvexInternalShape(), + m_localAabbMin(1, 1, 1), + m_localAabbMax(-1, -1, -1), + m_isLocalAabbValid(false) { } - -void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); + getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin()); } -void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling) +void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling) { btConvexInternalShape::setLocalScaling(scaling); recalcLocalAabb(); } - -void btConvexInternalAabbCachingShape::recalcLocalAabb() +void btConvexInternalAabbCachingShape::recalcLocalAabb() { m_isLocalAabbValid = true; - - #if 1 + +#if 1 static const btVector3 _directions[] = - { - btVector3( 1., 0., 0.), - btVector3( 0., 1., 0.), - btVector3( 0., 0., 1.), - btVector3( -1., 0., 0.), - btVector3( 0., -1., 0.), - btVector3( 0., 0., -1.) - }; - + { + btVector3(1., 0., 0.), + btVector3(0., 1., 0.), + btVector3(0., 0., 1.), + btVector3(-1., 0., 0.), + btVector3(0., -1., 0.), + btVector3(0., 0., -1.)}; + btVector3 _supporting[] = - { - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.) - }; - + { + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.)}; + batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); - - for ( int i = 0; i < 3; ++i ) + + for (int i = 0; i < 3; ++i) { - m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin; - m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin; + m_localAabbMax[i] = _supporting[i][i]; + m_localAabbMin[i] = _supporting[i + 3][i]; } - - #else - for (int i=0;i<3;i++) +#else + + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]+m_collisionMargin; + m_localAabbMax[i] = tmp[i]; vec[i] = btScalar(-1.); tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]-m_collisionMargin; + m_localAabbMin[i] = tmp[i]; } - #endif +#endif } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h index 37e04f5fc81..a28c57de4b8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -19,39 +19,35 @@ subject to the following restrictions: #include "btConvexShape.h" #include "LinearMath/btAabbUtil2.h" - ///The btConvexInternalShape is an internal base class, shared by most convex shape implementations. ///The btConvexInternalShape uses a default collision margin set to CONVEX_DISTANCE_MARGIN. ///This collision margin used by Gjk and some other algorithms, see also btCollisionMargin.h -///Note that when creating small shapes (derived from btConvexInternalShape), +///Note that when creating small shapes (derived from btConvexInternalShape), ///you need to make sure to set a smaller collision margin, using the 'setMargin' API ///There is a automatic mechanism 'setSafeMargin' used by btBoxShape and btCylinderShape -ATTRIBUTE_ALIGNED16(class) btConvexInternalShape : public btConvexShape +ATTRIBUTE_ALIGNED16(class) +btConvexInternalShape : public btConvexShape { - - protected: - +protected: //local scaling. collisionMargin is not scaled ! - btVector3 m_localScaling; + btVector3 m_localScaling; + + btVector3 m_implicitShapeDimensions; - btVector3 m_implicitShapeDimensions; - - btScalar m_collisionMargin; + btScalar m_collisionMargin; - btScalar m_padding; + btScalar m_padding; btConvexInternalShape(); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); virtual ~btConvexInternalShape() { - } - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; const btVector3& getImplicitShapeDimensions() const { @@ -62,163 +58,151 @@ public: ///changing a collision shape while the body is in the world is not recommended, ///it is best to remove the body from the world, then make the change, and re-add it ///alternatively flush the contact points, see documentation for 'cleanProxyFromPairs' - void setImplicitShapeDimensions(const btVector3& dimensions) + void setImplicitShapeDimensions(const btVector3& dimensions) { m_implicitShapeDimensions = dimensions; } - void setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f) + void setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f) { - btScalar safeMargin = defaultMarginMultiplier*minDimension; + btScalar safeMargin = defaultMarginMultiplier * minDimension; if (safeMargin < getMargin()) { setMargin(safeMargin); } } - void setSafeMargin(const btVector3& halfExtents, btScalar defaultMarginMultiplier = 0.1f) + void setSafeMargin(const btVector3& halfExtents, btScalar defaultMarginMultiplier = 0.1f) { //see http://code.google.com/p/bullet/issues/detail?id=349 //this margin check could could be added to other collision shapes too, //or add some assert/warning somewhere - btScalar minDimension=halfExtents[halfExtents.minAxis()]; + btScalar minDimension = halfExtents[halfExtents.minAxis()]; setSafeMargin(minDimension, defaultMarginMultiplier); } ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - getAabbSlow(t,aabbMin,aabbMax); + getAabbSlow(t, aabbMin, aabbMax); } + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - - virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const { return m_localScaling; } - const btVector3& getLocalScalingNV() const + const btVector3& getLocalScalingNV() const { return m_localScaling; } - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { m_collisionMargin = margin; } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { return m_collisionMargin; } - btScalar getMarginNV() const + btScalar getMarginNV() const { return m_collisionMargin; } - virtual int getNumPreferredPenetrationDirections() const + virtual int getNumPreferredPenetrationDirections() const { return 0; } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { (void)penetrationVector; (void)index; btAssert(0); } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btConvexInternalShapeData +struct btConvexInternalShapeData { - btCollisionShapeData m_collisionShapeData; + btCollisionShapeData m_collisionShapeData; - btVector3FloatData m_localScaling; + btVector3FloatData m_localScaling; - btVector3FloatData m_implicitShapeDimensions; - - float m_collisionMargin; + btVector3FloatData m_implicitShapeDimensions; - int m_padding; + float m_collisionMargin; + int m_padding; }; - - -SIMD_FORCE_INLINE int btConvexInternalShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btConvexInternalShape::calculateSerializeBufferSize() const { return sizeof(btConvexInternalShapeData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btConvexInternalShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*) dataBuffer; + btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*)dataBuffer; btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); m_implicitShapeDimensions.serializeFloat(shapeData->m_implicitShapeDimensions); m_localScaling.serializeFloat(shapeData->m_localScaling); shapeData->m_collisionMargin = float(m_collisionMargin); + // Fill padding with zeros to appease msan. + shapeData->m_padding = 0; + return "btConvexInternalShapeData"; } - - - ///btConvexInternalAabbCachingShape adds local aabb caching for convex shapes, to avoid expensive bounding box calculations class btConvexInternalAabbCachingShape : public btConvexInternalShape { - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - bool m_isLocalAabbValid; - + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; + protected: - btConvexInternalAabbCachingShape(); - - void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax) + + void setCachedLocalAabb(const btVector3& aabbMin, const btVector3& aabbMax) { m_isLocalAabbValid = true; m_localAabbMin = aabbMin; m_localAabbMax = aabbMax; } - inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const + inline void getCachedLocalAabb(btVector3& aabbMin, btVector3& aabbMax) const { btAssert(m_isLocalAabbValid); aabbMin = m_localAabbMin; aabbMax = m_localAabbMax; } - inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const + inline void getNonvirtualAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax, btScalar margin) const { - //lazy evaluation of local aabb btAssert(m_isLocalAabbValid); - btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax); + btTransformAabb(m_localAabbMin, m_localAabbMax, margin, trans, aabbMin, aabbMax); } - -public: - - virtual void setLocalScaling(const btVector3& scaling); - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; +public: + virtual void setLocalScaling(const btVector3& scaling); - void recalcLocalAabb(); + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + void recalcLocalAabb(); }; -#endif //BT_CONVEX_INTERNAL_SHAPE_H +#endif //BT_CONVEX_INTERNAL_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp index ad1d1bf78f8..f00a440fa35 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp @@ -25,81 +25,73 @@ void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling) } #ifndef __SPU__ -btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.)); btScalar maxDot = btScalar(-BT_LARGE_FLOAT); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } - - if( m_numPoints > 0 ) - { - // Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically. - // btVector3 scaled = vec * m_localScaling; - int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot); //FIXME: may violate encapsulation of m_unscaledPoints - return getScaledPoint(index); - } + + if (m_numPoints > 0) + { + // Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically. + // btVector3 scaled = vec * m_localScaling; + int index = (int)vec.maxDot(&m_unscaledPoints[0], m_numPoints, maxDot); //FIXME: may violate encapsulation of m_unscaledPoints + return getScaledPoint(index); + } return supVec; } -void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for( int j = 0; j < numVectors; j++ ) - { - const btVector3& vec = vectors[j] * m_localScaling; // dot( a*c, b) = dot(a, b*c) - btScalar maxDot; - int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot); - supportVerticesOut[j][3] = btScalar(-BT_LARGE_FLOAT); - if( 0 <= index ) - { - //WARNING: don't swap next lines, the w component would get overwritten! - supportVerticesOut[j] = getScaledPoint(index); - supportVerticesOut[j][3] = maxDot; - } - } - + for (int j = 0; j < numVectors; j++) + { + const btVector3& vec = vectors[j] * m_localScaling; // dot( a*c, b) = dot(a, b*c) + btScalar maxDot; + int index = (int)vec.maxDot(&m_unscaledPoints[0], m_numPoints, maxDot); + supportVerticesOut[j][3] = btScalar(-BT_LARGE_FLOAT); + if (0 <= index) + { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j] = getScaledPoint(index); + supportVerticesOut[j][3] = maxDot; + } + } } - - -btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - #endif - - - - - //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo -int btConvexPointCloudShape::getNumVertices() const +int btConvexPointCloudShape::getNumVertices() const { return m_numPoints; } @@ -109,31 +101,29 @@ int btConvexPointCloudShape::getNumEdges() const return 0; } -void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const +void btConvexPointCloudShape::getEdge(int i, btVector3& pa, btVector3& pb) const { - btAssert (0); + btAssert(0); } -void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const +void btConvexPointCloudShape::getVertex(int i, btVector3& vtx) const { - vtx = m_unscaledPoints[i]*m_localScaling; + vtx = m_unscaledPoints[i] * m_localScaling; } -int btConvexPointCloudShape::getNumPlanes() const +int btConvexPointCloudShape::getNumPlanes() const { return 0; } -void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const +void btConvexPointCloudShape::getPlane(btVector3&, btVector3&, int) const { - btAssert(0); } //not yet -bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const +bool btConvexPointCloudShape::isInside(const btVector3&, btScalar) const { btAssert(0); return false; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h index 54b5afac3ec..c7d554a4d3d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h @@ -17,11 +17,12 @@ subject to the following restrictions: #define BT_CONVEX_POINT_CLOUD_SHAPE_H #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btAlignedObjectArray.h" ///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices. -ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape { btVector3* m_unscaledPoints; int m_numPoints; @@ -31,13 +32,13 @@ public: btConvexPointCloudShape() { - m_localScaling.setValue(1.f,1.f,1.f); + m_localScaling.setValue(1.f, 1.f, 1.f); m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; m_unscaledPoints = 0; m_numPoints = 0; } - btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true) + btConvexPointCloudShape(btVector3 * points, int numPoints, const btVector3& localScaling, bool computeAabb = true) { m_localScaling = localScaling; m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; @@ -48,7 +49,7 @@ public: recalcLocalAabb(); } - void setPoints (btVector3* points, int numPoints, bool computeAabb = true,const btVector3& localScaling=btVector3(1.f,1.f,1.f)) + void setPoints(btVector3 * points, int numPoints, bool computeAabb = true, const btVector3& localScaling = btVector3(1.f, 1.f, 1.f)) { m_unscaledPoints = points; m_numPoints = numPoints; @@ -58,48 +59,45 @@ public: recalcLocalAabb(); } - SIMD_FORCE_INLINE btVector3* getUnscaledPoints() + SIMD_FORCE_INLINE btVector3* getUnscaledPoints() { return m_unscaledPoints; } - SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const + SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const { return m_unscaledPoints; } - SIMD_FORCE_INLINE int getNumPoints() const + SIMD_FORCE_INLINE int getNumPoints() const { return m_numPoints; } - SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const + SIMD_FORCE_INLINE btVector3 getScaledPoint(int index) const { return m_unscaledPoints[index] * m_localScaling; } #ifndef __SPU__ - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; #endif - //debugging - virtual const char* getName()const {return "ConvexPointCloud";} + virtual const char* getName() const { return "ConvexPointCloud"; } - virtual int getNumVertices() const; + virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; + virtual void getVertex(int i, btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; ///in case we receive negative scaling - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); }; - -#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H - +#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index 4f45319a83a..9694f4ddb3d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans ///Separating axis rest based on work from Pierre Terdiman, see ///And contact clipping based on work from Simon Hobbs @@ -21,49 +20,45 @@ subject to the following restrictions: #include "btConvexPolyhedron.h" #include "LinearMath/btHashMap.h" - btConvexPolyhedron::btConvexPolyhedron() { - } btConvexPolyhedron::~btConvexPolyhedron() { - } - -inline bool IsAlmostZero(const btVector3& v) +inline bool IsAlmostZero1(const btVector3& v) { - if(btFabs(v.x())>1e-6 || btFabs(v.y())>1e-6 || btFabs(v.z())>1e-6) return false; + if (btFabs(v.x()) > 1e-6 || btFabs(v.y()) > 1e-6 || btFabs(v.z()) > 1e-6) return false; return true; } struct btInternalVertexPair { - btInternalVertexPair(short int v0,short int v1) - :m_v0(v0), - m_v1(v1) + btInternalVertexPair(short int v0, short int v1) + : m_v0(v0), + m_v1(v1) { - if (m_v1>m_v0) - btSwap(m_v0,m_v1); + if (m_v1 > m_v0) + btSwap(m_v0, m_v1); } short int m_v0; short int m_v1; int getHash() const { - return m_v0+(m_v1<<16); + return m_v0 + (m_v1 << 16); } bool equals(const btInternalVertexPair& other) const { - return m_v0==other.m_v0 && m_v1==other.m_v1; + return m_v0 == other.m_v0 && m_v1 == other.m_v1; } }; struct btInternalEdge { btInternalEdge() - :m_face0(-1), - m_face1(-1) + : m_face0(-1), + m_face1(-1) { } short int m_face0; @@ -75,23 +70,31 @@ struct btInternalEdge #ifdef TEST_INTERNAL_OBJECTS bool btConvexPolyhedron::testContainment() const { - for(int p=0;p<8;p++) + for (int p = 0; p < 8; p++) { btVector3 LocalPt; - if(p==0) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]); - else if(p==1) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]); - else if(p==2) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]); - else if(p==3) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]); - else if(p==4) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]); - else if(p==5) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]); - else if(p==6) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]); - else if(p==7) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]); - - for(int i=0;i<m_faces.size();i++) + if (p == 0) + LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]); + else if (p == 1) + LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]); + else if (p == 2) + LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]); + else if (p == 3) + LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]); + else if (p == 4) + LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]); + else if (p == 5) + LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]); + else if (p == 6) + LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]); + else if (p == 7) + LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]); + + for (int i = 0; i < m_faces.size(); i++) { const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]); const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3]; - if(d>0.0f) + if (d > 0.0f) return false; } } @@ -99,33 +102,28 @@ bool btConvexPolyhedron::testContainment() const } #endif -void btConvexPolyhedron::initialize() +void btConvexPolyhedron::initialize() { + btHashMap<btInternalVertexPair, btInternalEdge> edges; - btHashMap<btInternalVertexPair,btInternalEdge> edges; - - btScalar TotalArea = 0.0f; - - m_localCenter.setValue(0, 0, 0); - for(int i=0;i<m_faces.size();i++) + for (int i = 0; i < m_faces.size(); i++) { int numVertices = m_faces[i].m_indices.size(); int NbTris = numVertices; - for(int j=0;j<NbTris;j++) + for (int j = 0; j < NbTris; j++) { - int k = (j+1)%numVertices; - btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]); + int k = (j + 1) % numVertices; + btInternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]); btInternalEdge* edptr = edges.find(vp); - btVector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0]; + btVector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0]; edge.normalize(); bool found = false; - for (int p=0;p<m_uniqueEdges.size();p++) + for (int p = 0; p < m_uniqueEdges.size(); p++) { - - if (IsAlmostZero(m_uniqueEdges[p]-edge) || - IsAlmostZero(m_uniqueEdges[p]+edge)) + if (IsAlmostZero1(m_uniqueEdges[p] - edge) || + IsAlmostZero1(m_uniqueEdges[p] + edge)) { found = true; break; @@ -139,105 +137,107 @@ void btConvexPolyhedron::initialize() if (edptr) { - btAssert(edptr->m_face0>=0); - btAssert(edptr->m_face1<0); + btAssert(edptr->m_face0 >= 0); + btAssert(edptr->m_face1 < 0); edptr->m_face1 = i; - } else + } + else { btInternalEdge ed; ed.m_face0 = i; - edges.insert(vp,ed); + edges.insert(vp, ed); } } } #ifdef USE_CONNECTED_FACES - for(int i=0;i<m_faces.size();i++) + for (int i = 0; i < m_faces.size(); i++) { int numVertices = m_faces[i].m_indices.size(); m_faces[i].m_connectedFaces.resize(numVertices); - for(int j=0;j<numVertices;j++) + for (int j = 0; j < numVertices; j++) { - int k = (j+1)%numVertices; - btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]); + int k = (j + 1) % numVertices; + btInternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]); btInternalEdge* edptr = edges.find(vp); btAssert(edptr); - btAssert(edptr->m_face0>=0); - btAssert(edptr->m_face1>=0); + btAssert(edptr->m_face0 >= 0); + btAssert(edptr->m_face1 >= 0); - int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0; + int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0; m_faces[i].m_connectedFaces[j] = connectedFace; } } -#endif//USE_CONNECTED_FACES +#endif //USE_CONNECTED_FACES + + initialize2(); +} - for(int i=0;i<m_faces.size();i++) +void btConvexPolyhedron::initialize2() +{ + m_localCenter.setValue(0, 0, 0); + btScalar TotalArea = 0.0f; + for (int i = 0; i < m_faces.size(); i++) { int numVertices = m_faces[i].m_indices.size(); - int NbTris = numVertices-2; - + int NbTris = numVertices - 2; + const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]]; - for(int j=1;j<=NbTris;j++) + for (int j = 1; j <= NbTris; j++) { - int k = (j+1)%numVertices; + int k = (j + 1) % numVertices; const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]]; const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]]; btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f; - btVector3 Center = (p0+p1+p2)/3.0f; + btVector3 Center = (p0 + p1 + p2) / 3.0f; m_localCenter += Area * Center; TotalArea += Area; } } m_localCenter /= TotalArea; - - - #ifdef TEST_INTERNAL_OBJECTS - if(1) + if (1) { m_radius = FLT_MAX; - for(int i=0;i<m_faces.size();i++) + for (int i = 0; i < m_faces.size(); i++) { const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]); const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]); - if(dist<m_radius) + if (dist < m_radius) m_radius = dist; } - btScalar MinX = FLT_MAX; btScalar MinY = FLT_MAX; btScalar MinZ = FLT_MAX; btScalar MaxX = -FLT_MAX; btScalar MaxY = -FLT_MAX; btScalar MaxZ = -FLT_MAX; - for(int i=0; i<m_vertices.size(); i++) + for (int i = 0; i < m_vertices.size(); i++) { const btVector3& pt = m_vertices[i]; - if(pt.x()<MinX) MinX = pt.x(); - if(pt.x()>MaxX) MaxX = pt.x(); - if(pt.y()<MinY) MinY = pt.y(); - if(pt.y()>MaxY) MaxY = pt.y(); - if(pt.z()<MinZ) MinZ = pt.z(); - if(pt.z()>MaxZ) MaxZ = pt.z(); + if (pt.x() < MinX) MinX = pt.x(); + if (pt.x() > MaxX) MaxX = pt.x(); + if (pt.y() < MinY) MinY = pt.y(); + if (pt.y() > MaxY) MaxY = pt.y(); + if (pt.z() < MinZ) MinZ = pt.z(); + if (pt.z() > MaxZ) MaxZ = pt.z(); } - mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ); - mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ); - + mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ); + mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ); - -// const btScalar r = m_radius / sqrtf(2.0f); + // const btScalar r = m_radius / sqrtf(2.0f); const btScalar r = m_radius / sqrtf(3.0f); const int LargestExtent = mE.maxAxis(); - const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f; + const btScalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f; m_extents[0] = m_extents[1] = m_extents[2] = r; - m_extents[LargestExtent] = mE[LargestExtent]*0.5f; + m_extents[LargestExtent] = mE[LargestExtent] * 0.5f; bool FoundBox = false; - for(int j=0;j<1024;j++) + for (int j = 0; j < 1024; j++) { - if(testContainment()) + if (testContainment()) { FoundBox = true; break; @@ -245,25 +245,25 @@ void btConvexPolyhedron::initialize() m_extents[LargestExtent] -= Step; } - if(!FoundBox) + if (!FoundBox) { m_extents[0] = m_extents[1] = m_extents[2] = r; } else { // Refine the box - const btScalar Step = (m_radius - r)/1024.0f; - const int e0 = (1<<LargestExtent) & 3; - const int e1 = (1<<e0) & 3; + const btScalar Step = (m_radius - r) / 1024.0f; + const int e0 = (1 << LargestExtent) & 3; + const int e1 = (1 << e0) & 3; - for(int j=0;j<1024;j++) + for (int j = 0; j < 1024; j++) { const btScalar Saved0 = m_extents[e0]; const btScalar Saved1 = m_extents[e1]; m_extents[e0] += Step; m_extents[e1] += Step; - if(!testContainment()) + if (!testContainment()) { m_extents[e0] = Saved0; m_extents[e1] = Saved1; @@ -274,30 +274,29 @@ void btConvexPolyhedron::initialize() } #endif } - -void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const +void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const { minProj = FLT_MAX; maxProj = -FLT_MAX; int numVerts = m_vertices.size(); - for(int i=0;i<numVerts;i++) + for (int i = 0; i < numVerts; i++) { btVector3 pt = trans * m_vertices[i]; btScalar dp = pt.dot(dir); - if(dp < minProj) + if (dp < minProj) { minProj = dp; witnesPtMin = pt; } - if(dp > maxProj) + if (dp > maxProj) { maxProj = dp; witnesPtMax = pt; } } - if(minProj>maxProj) + if (minProj > maxProj) { - btSwap(minProj,maxProj); - btSwap(witnesPtMin,witnesPtMax); + btSwap(minProj, maxProj); + btSwap(witnesPtMin, witnesPtMax); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h index d3cd066ac8d..638aa9b3df0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h @@ -13,10 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - ///This file was written by Erwin Coumans - #ifndef _BT_POLYHEDRAL_FEATURES_H #define _BT_POLYHEDRAL_FEATURES_H @@ -25,41 +23,37 @@ subject to the following restrictions: #define TEST_INTERNAL_OBJECTS 1 - struct btFace { - btAlignedObjectArray<int> m_indices; -// btAlignedObjectArray<int> m_connectedFaces; - btScalar m_plane[4]; + btAlignedObjectArray<int> m_indices; + // btAlignedObjectArray<int> m_connectedFaces; + btScalar m_plane[4]; }; - -ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron +ATTRIBUTE_ALIGNED16(class) +btConvexPolyhedron { - public: - +public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btConvexPolyhedron(); - virtual ~btConvexPolyhedron(); + virtual ~btConvexPolyhedron(); - btAlignedObjectArray<btVector3> m_vertices; - btAlignedObjectArray<btFace> m_faces; + btAlignedObjectArray<btVector3> m_vertices; + btAlignedObjectArray<btFace> m_faces; btAlignedObjectArray<btVector3> m_uniqueEdges; - btVector3 m_localCenter; - btVector3 m_extents; - btScalar m_radius; - btVector3 mC; - btVector3 mE; + btVector3 m_localCenter; + btVector3 m_extents; + btScalar m_radius; + btVector3 mC; + btVector3 mE; - void initialize(); + void initialize(); + void initialize2(); bool testContainment() const; - void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; + void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const; }; - -#endif //_BT_POLYHEDRAL_FEATURES_H - - +#endif //_BT_POLYHEDRAL_FEATURES_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index 88018b4c624..f8fb0aa9fda 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -13,9 +13,9 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//#if defined (_WIN32) || defined (__i386__) -//#define BT_USE_SSE_IN_API -//#endif +#if defined(_WIN32) || defined(__i386__) +#define BT_USE_SSE_IN_API +#endif #include "btConvexShape.h" #include "btTriangleShape.h" @@ -27,30 +27,28 @@ subject to the following restrictions: #include "btConvexPointCloudShape.h" ///not supported on IBM SDK, until we fix the alignment of btVector3 -#if defined (__CELLOS_LV2__) && defined (__SPU__) +#if defined(__CELLOS_LV2__) && defined(__SPU__) #include <spu_intrinsics.h> -static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) +static inline vec_float4 vec_dot3(vec_float4 vec0, vec_float4 vec1) { - vec_float4 result; - result = spu_mul( vec0, vec1 ); - result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result ); - return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result ); + vec_float4 result; + result = spu_mul(vec0, vec1); + result = spu_madd(spu_rlqwbyte(vec0, 4), spu_rlqwbyte(vec1, 4), result); + return spu_madd(spu_rlqwbyte(vec0, 8), spu_rlqwbyte(vec1, 8), result); } -#endif //__SPU__ +#endif //__SPU__ -btConvexShape::btConvexShape () +btConvexShape::btConvexShape() { } btConvexShape::~btConvexShape() { - } - -void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin,btVector3& witnesPtMax) const +void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max, btVector3& witnesPtMin, btVector3& witnesPtMax) const { - btVector3 localAxis = dir*trans.getBasis(); + btVector3 localAxis = dir * trans.getBasis(); btVector3 vtx1 = trans(localGetSupportingVertex(localAxis)); btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis)); @@ -58,8 +56,8 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc max = vtx2.dot(dir); witnesPtMax = vtx2; witnesPtMin = vtx1; - - if(min>max) + + if (min > max) { btScalar tmp = min; min = max; @@ -69,391 +67,392 @@ void btConvexShape::project(const btTransform& trans, const btVector3& dir, btSc } } - -static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) -{ - +static btVector3 convexHullSupport(const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) +{ btVector3 vec = localDirOrg * localScaling; -#if defined (__CELLOS_LV2__) && defined (__SPU__) +#if defined(__CELLOS_LV2__) && defined(__SPU__) btVector3 localDir = vec; - vec_float4 v_distMax = {-FLT_MAX,0,0,0}; - vec_int4 v_idxMax = {-999,0,0,0}; - int v=0; + vec_float4 v_distMax = {-FLT_MAX, 0, 0, 0}; + vec_int4 v_idxMax = {-999, 0, 0, 0}; + int v = 0; int numverts = numPoints; - for(;v<(int)numverts-4;v+=4) { - vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); - vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); - vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); - vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); - const vec_int4 i0 = {v ,0,0,0}; - const vec_int4 i1 = {v+1,0,0,0}; - const vec_int4 i2 = {v+2,0,0,0}; - const vec_int4 i3 = {v+3,0,0,0}; - vec_uint4 retGt01 = spu_cmpgt(p0,p1); - vec_float4 pmax01 = spu_sel(p1,p0,retGt01); - vec_int4 imax01 = spu_sel(i1,i0,retGt01); - vec_uint4 retGt23 = spu_cmpgt(p2,p3); - vec_float4 pmax23 = spu_sel(p3,p2,retGt23); - vec_int4 imax23 = spu_sel(i3,i2,retGt23); - vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); - vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); - vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); - vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); - v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); - v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); + for (; v < (int)numverts - 4; v += 4) + { + vec_float4 p0 = vec_dot3(points[v].get128(), localDir.get128()); + vec_float4 p1 = vec_dot3(points[v + 1].get128(), localDir.get128()); + vec_float4 p2 = vec_dot3(points[v + 2].get128(), localDir.get128()); + vec_float4 p3 = vec_dot3(points[v + 3].get128(), localDir.get128()); + const vec_int4 i0 = {v, 0, 0, 0}; + const vec_int4 i1 = {v + 1, 0, 0, 0}; + const vec_int4 i2 = {v + 2, 0, 0, 0}; + const vec_int4 i3 = {v + 3, 0, 0, 0}; + vec_uint4 retGt01 = spu_cmpgt(p0, p1); + vec_float4 pmax01 = spu_sel(p1, p0, retGt01); + vec_int4 imax01 = spu_sel(i1, i0, retGt01); + vec_uint4 retGt23 = spu_cmpgt(p2, p3); + vec_float4 pmax23 = spu_sel(p3, p2, retGt23); + vec_int4 imax23 = spu_sel(i3, i2, retGt23); + vec_uint4 retGt0123 = spu_cmpgt(pmax01, pmax23); + vec_float4 pmax0123 = spu_sel(pmax23, pmax01, retGt0123); + vec_int4 imax0123 = spu_sel(imax23, imax01, retGt0123); + vec_uint4 retGtMax = spu_cmpgt(v_distMax, pmax0123); + v_distMax = spu_sel(pmax0123, v_distMax, retGtMax); + v_idxMax = spu_sel(imax0123, v_idxMax, retGtMax); } - for(;v<(int)numverts;v++) { - vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); - const vec_int4 i = {v,0,0,0}; - vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); - v_distMax = spu_sel(p,v_distMax,retGtMax); - v_idxMax = spu_sel(i,v_idxMax,retGtMax); + for (; v < (int)numverts; v++) + { + vec_float4 p = vec_dot3(points[v].get128(), localDir.get128()); + const vec_int4 i = {v, 0, 0, 0}; + vec_uint4 retGtMax = spu_cmpgt(v_distMax, p); + v_distMax = spu_sel(p, v_distMax, retGtMax); + v_idxMax = spu_sel(i, v_idxMax, retGtMax); } - int ptIndex = spu_extract(v_idxMax,0); - const btVector3& supVec= points[ptIndex] * localScaling; + int ptIndex = spu_extract(v_idxMax, 0); + const btVector3& supVec = points[ptIndex] * localScaling; return supVec; #else - btScalar maxDot; - long ptIndex = vec.maxDot( points, numPoints, maxDot); + btScalar maxDot; + long ptIndex = vec.maxDot(points, numPoints, maxDot); btAssert(ptIndex >= 0); + if (ptIndex < 0) + { + ptIndex = 0; + } btVector3 supVec = points[ptIndex] * localScaling; return supVec; -#endif //__SPU__ +#endif //__SPU__ } -btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const +btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual(const btVector3& localDir) const { switch (m_shapeType) { - case SPHERE_SHAPE_PROXYTYPE: - { - return btVector3(0,0,0); - } - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); - -#if defined( __APPLE__ ) && (defined( BT_USE_SSE )||defined( BT_USE_NEON )) - #if defined( BT_USE_SSE ) - return btVector3( _mm_xor_ps( _mm_and_ps( localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f }), halfExtents.mVec128 )); - #elif defined( BT_USE_NEON ) - return btVector3( (float32x4_t) (((uint32x4_t) localDir.mVec128 & (uint32x4_t){ 0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t) halfExtents.mVec128 )); - #else - #error unknown vector arch - #endif -#else - return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), - btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), - btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); -#endif - } - case TRIANGLE_SHAPE_PROXYTYPE: - { - btTriangleShape* triangleShape = (btTriangleShape*)this; - btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3* vertices = &triangleShape->m_vertices1[0]; - btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]); - btVector3 sup = vertices[dots.maxAxis()]; - return btVector3(sup.getX(),sup.getY(),sup.getZ()); - } - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShape* cylShape = (btCylinderShape*)this; - //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) - - btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); - btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); - int cylinderUpAxis = cylShape->getUpAxis(); - int XX(1),YY(0),ZZ(2); - - switch (cylinderUpAxis) + case SPHERE_SHAPE_PROXYTYPE: { - case 0: - { - XX = 1; - YY = 0; - ZZ = 2; + return btVector3(0, 0, 0); } - break; - case 1: + case BOX_SHAPE_PROXYTYPE: { - XX = 0; - YY = 1; - ZZ = 2; + btBoxShape* convexShape = (btBoxShape*)this; + const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); + +#if defined(__APPLE__) && (defined(BT_USE_SSE) || defined(BT_USE_NEON)) +#if defined(BT_USE_SSE) + return btVector3(_mm_xor_ps(_mm_and_ps(localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f}), halfExtents.mVec128)); +#elif defined(BT_USE_NEON) + return btVector3((float32x4_t)(((uint32x4_t)localDir.mVec128 & (uint32x4_t){0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t)halfExtents.mVec128)); +#else +#error unknown vector arch +#endif +#else + return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()), + btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()), + btFsels(localDir.z(), halfExtents.z(), -halfExtents.z())); +#endif } - break; - case 2: + case TRIANGLE_SHAPE_PROXYTYPE: { - XX = 0; - YY = 2; - ZZ = 1; - + btTriangleShape* triangleShape = (btTriangleShape*)this; + btVector3 dir(localDir.getX(), localDir.getY(), localDir.getZ()); + btVector3* vertices = &triangleShape->m_vertices1[0]; + btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]); + btVector3 sup = vertices[dots.maxAxis()]; + return btVector3(sup.getX(), sup.getY(), sup.getZ()); } - break; - default: - btAssert(0); - break; - }; - - btScalar radius = halfExtents[XX]; - btScalar halfHeight = halfExtents[cylinderUpAxis]; + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShape* cylShape = (btCylinderShape*)this; + //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) - btVector3 tmp; - btScalar d ; + btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); + btVector3 v(localDir.getX(), localDir.getY(), localDir.getZ()); + int cylinderUpAxis = cylShape->getUpAxis(); + int XX(1), YY(0), ZZ(2); - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) - { - d = radius / s; - tmp[XX] = v[XX] * d; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = v[ZZ] * d; - return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); - } else { - tmp[XX] = radius; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = btScalar(0.0); - return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); + switch (cylinderUpAxis) + { + case 0: + { + XX = 1; + YY = 0; + ZZ = 2; + } + break; + case 1: + { + XX = 0; + YY = 1; + ZZ = 2; + } + break; + case 2: + { + XX = 0; + YY = 2; + ZZ = 1; + } + break; + default: + btAssert(0); + break; + }; + + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; + + btVector3 tmp; + btScalar d; + + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) + { + d = radius / s; + tmp[XX] = v[XX] * d; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = v[ZZ] * d; + return btVector3(tmp.getX(), tmp.getY(), tmp.getZ()); + } + else + { + tmp[XX] = radius; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = btScalar(0.0); + return btVector3(tmp.getX(), tmp.getY(), tmp.getZ()); + } } - } - case CAPSULE_SHAPE_PROXYTYPE: - { - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); + case CAPSULE_SHAPE_PROXYTYPE: + { + btVector3 vec0(localDir.getX(), localDir.getY(), localDir.getZ()); - btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - btScalar halfHeight = capsuleShape->getHalfHeight(); - int capsuleUpAxis = capsuleShape->getUpAxis(); + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + btScalar halfHeight = capsuleShape->getHalfHeight(); + int capsuleUpAxis = capsuleShape->getUpAxis(); - btScalar radius = capsuleShape->getRadius(); - btVector3 supVec(0,0,0); + btVector3 supVec(0, 0, 0); - btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); + btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - btVector3 vtx; - btScalar newDot; - { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = halfHeight; + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < SIMD_EPSILON * SIMD_EPSILON) + { + vec.setValue(1, 0, 0); + } + else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); + vec *= rlen; + } + btVector3 vtx; + btScalar newDot; + { + btVector3 pos(0, 0, 0); + pos[capsuleUpAxis] = halfHeight; - //vtx = pos +vec*(radius); - vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV(); - newDot = vec.dot(vtx); - + vtx = pos; + newDot = vec.dot(vtx); - if (newDot > maxDot) + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } { - maxDot = newDot; - supVec = vtx; + btVector3 pos(0, 0, 0); + pos[capsuleUpAxis] = -halfHeight; + + vtx = pos; + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } } + return btVector3(supVec.getX(), supVec.getY(), supVec.getZ()); } + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = -halfHeight; - - //vtx = pos +vec*(radius); - vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV(); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } + btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; + btVector3* points = convexPointCloudShape->getUnscaledPoints(); + int numPoints = convexPointCloudShape->getNumPoints(); + return convexHullSupport(localDir, points, numPoints, convexPointCloudShape->getLocalScalingNV()); } - return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); - } - case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: - { - btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; - btVector3* points = convexPointCloudShape->getUnscaledPoints (); - int numPoints = convexPointCloudShape->getNumPoints (); - return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV()); - } - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - btConvexHullShape* convexHullShape = (btConvexHullShape*)this; - btVector3* points = convexHullShape->getUnscaledPoints(); - int numPoints = convexHullShape->getNumPoints (); - return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV()); - } - default: + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btConvexHullShape* convexHullShape = (btConvexHullShape*)this; + btVector3* points = convexHullShape->getUnscaledPoints(); + int numPoints = convexHullShape->getNumPoints(); + return convexHullSupport(localDir, points, numPoints, convexHullShape->getLocalScalingNV()); + } + default: #ifndef __SPU__ - return this->localGetSupportingVertexWithoutMargin (localDir); + return this->localGetSupportingVertexWithoutMargin(localDir); #else - btAssert (0); + btAssert(0); #endif } // should never reach here - btAssert (0); - return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); + btAssert(0); + return btVector3(btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); } -btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const +btVector3 btConvexShape::localGetSupportVertexNonVirtual(const btVector3& localDir) const { btVector3 localDirNorm = localDir; - if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (localDirNorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); + localDirNorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); } - localDirNorm.normalize (); + localDirNorm.normalize(); - return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm; + return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm) + getMarginNonVirtual() * localDirNorm; } /* TODO: This should be bumped up to btCollisionShape () */ -btScalar btConvexShape::getMarginNonVirtual () const +btScalar btConvexShape::getMarginNonVirtual() const { switch (m_shapeType) { - case SPHERE_SHAPE_PROXYTYPE: - { - btSphereShape* sphereShape = (btSphereShape*)this; - return sphereShape->getRadius (); - } - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - return convexShape->getMarginNV (); - } - case TRIANGLE_SHAPE_PROXYTYPE: - { - btTriangleShape* triangleShape = (btTriangleShape*)this; - return triangleShape->getMarginNV (); - } - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShape* cylShape = (btCylinderShape*)this; - return cylShape->getMarginNV(); - } - case CONE_SHAPE_PROXYTYPE: - { - btConeShape* conShape = (btConeShape*)this; - return conShape->getMarginNV(); - } - case CAPSULE_SHAPE_PROXYTYPE: - { - btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - return capsuleShape->getMarginNV(); - } - case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: - /* fall through */ - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; - return convexHullShape->getMarginNV(); - } - default: + case SPHERE_SHAPE_PROXYTYPE: + { + btSphereShape* sphereShape = (btSphereShape*)this; + return sphereShape->getRadius(); + } + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* convexShape = (btBoxShape*)this; + return convexShape->getMarginNV(); + } + case TRIANGLE_SHAPE_PROXYTYPE: + { + btTriangleShape* triangleShape = (btTriangleShape*)this; + return triangleShape->getMarginNV(); + } + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShape* cylShape = (btCylinderShape*)this; + return cylShape->getMarginNV(); + } + case CONE_SHAPE_PROXYTYPE: + { + btConeShape* conShape = (btConeShape*)this; + return conShape->getMarginNV(); + } + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + return capsuleShape->getMarginNV(); + } + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + /* fall through */ + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btPolyhedralConvexShape* convexHullShape = (btPolyhedralConvexShape*)this; + return convexHullShape->getMarginNV(); + } + default: #ifndef __SPU__ - return this->getMargin (); + return this->getMargin(); #else - btAssert (0); + btAssert(0); #endif } // should never reach here - btAssert (0); + btAssert(0); return btScalar(0.0f); } #ifndef __SPU__ -void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const +void btConvexShape::getAabbNonVirtual(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { switch (m_shapeType) { - case SPHERE_SHAPE_PROXYTYPE: - { - btSphereShape* sphereShape = (btSphereShape*)this; - btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); - btScalar margin = radius + sphereShape->getMarginNonVirtual(); - const btVector3& center = t.getOrigin(); - btVector3 extent(margin,margin,margin); - aabbMin = center - extent; - aabbMax = center + extent; - } - break; - case CYLINDER_SHAPE_PROXYTYPE: - /* fall through */ - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - btScalar margin=convexShape->getMarginNonVirtual(); - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - halfExtents += btVector3(margin,margin,margin); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - - aabbMin = center - extent; - aabbMax = center + extent; + case SPHERE_SHAPE_PROXYTYPE: + { + btSphereShape* sphereShape = (btSphereShape*)this; + btScalar radius = sphereShape->getImplicitShapeDimensions().getX(); // * convexShape->getLocalScaling().getX(); + btScalar margin = radius + sphereShape->getMarginNonVirtual(); + const btVector3& center = t.getOrigin(); + btVector3 extent(margin, margin, margin); + aabbMin = center - extent; + aabbMax = center + extent; + } break; - } - case TRIANGLE_SHAPE_PROXYTYPE: - { - btTriangleShape* triangleShape = (btTriangleShape*)this; - btScalar margin = triangleShape->getMarginNonVirtual(); - for (int i=0;i<3;i++) + case CYLINDER_SHAPE_PROXYTYPE: + /* fall through */ + case BOX_SHAPE_PROXYTYPE: { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); - vec[i] = btScalar(1.); + btBoxShape* convexShape = (btBoxShape*)this; + btScalar margin = convexShape->getMarginNonVirtual(); + btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); + halfExtents += btVector3(margin, margin, margin); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + + aabbMin = center - extent; + aabbMax = center + extent; + break; + } + case TRIANGLE_SHAPE_PROXYTYPE: + { + btTriangleShape* triangleShape = (btTriangleShape*)this; + btScalar margin = triangleShape->getMarginNonVirtual(); + for (int i = 0; i < 3; i++) + { + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); + vec[i] = btScalar(1.); - btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()); + btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec * t.getBasis()); - btVector3 tmp = t(sv); - aabbMax[i] = tmp[i]+margin; - vec[i] = btScalar(-1.); - tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis())); - aabbMin[i] = tmp[i]-margin; - } - } - break; - case CAPSULE_SHAPE_PROXYTYPE: - { - btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius()); - int m_upAxis = capsuleShape->getUpAxis(); - halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); - halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual()); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - aabbMin = center - extent; - aabbMax = center + extent; - } - break; - case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this; - btScalar margin = convexHullShape->getMarginNonVirtual(); - convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin); - } - break; - default: + btVector3 tmp = t(sv); + aabbMax[i] = tmp[i] + margin; + vec[i] = btScalar(-1.); + tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec * t.getBasis())); + aabbMin[i] = tmp[i] - margin; + } + } + break; + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShape* capsuleShape = (btCapsuleShape*)this; + btVector3 halfExtents(capsuleShape->getRadius(), capsuleShape->getRadius(), capsuleShape->getRadius()); + int m_upAxis = capsuleShape->getUpAxis(); + halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + aabbMin = center - extent; + aabbMax = center + extent; + } + break; + case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this; + btScalar margin = convexHullShape->getMarginNonVirtual(); + convexHullShape->getNonvirtualAabb(t, aabbMin, aabbMax, margin); + } + break; + default: #ifndef __SPU__ - this->getAabb (t, aabbMin, aabbMax); + this->getAabb(t, aabbMin, aabbMax); #else - btAssert (0); + btAssert(0); #endif - break; + break; } // should never reach here - btAssert (0); + btAssert(0); } -#endif //__SPU__ +#endif //__SPU__ diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h index 875f2ac195a..d3b3ed816ec 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -28,58 +28,48 @@ subject to the following restrictions: /// The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape, btConvexHullShape etc. /// It describes general convex shapes using the localGetSupportingVertex interface, used by collision detectors such as btGjkPairDetector. -ATTRIBUTE_ALIGNED16(class) btConvexShape : public btCollisionShape +ATTRIBUTE_ALIGNED16(class) +btConvexShape : public btCollisionShape { - - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btConvexShape (); + btConvexShape(); virtual ~btConvexShape(); - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const = 0; - - //////// - #ifndef __SPU__ - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const=0; - #endif //#ifndef __SPU__ + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const = 0; - btVector3 localGetSupportVertexWithoutMarginNonVirtual (const btVector3& vec) const; - btVector3 localGetSupportVertexNonVirtual (const btVector3& vec) const; - btScalar getMarginNonVirtual () const; - void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; +//////// +#ifndef __SPU__ + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const = 0; +#endif //#ifndef __SPU__ + btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3& vec) const; + btVector3 localGetSupportVertexNonVirtual(const btVector3& vec) const; + btScalar getMarginNonVirtual() const; + void getAabbNonVirtual(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; + virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin, btVector3& witnesPtMax) const; - //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const = 0; ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0; - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const = 0; - virtual void setLocalScaling(const btVector3& scaling) =0; - virtual const btVector3& getLocalScaling() const =0; + virtual void setLocalScaling(const btVector3& scaling) = 0; + virtual const btVector3& getLocalScaling() const = 0; - virtual void setMargin(btScalar margin)=0; + virtual void setMargin(btScalar margin) = 0; - virtual btScalar getMargin() const=0; + virtual btScalar getMargin() const = 0; - virtual int getNumPreferredPenetrationDirections() const=0; - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0; + virtual int getNumPreferredPenetrationDirections() const = 0; - - - + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const = 0; }; - - -#endif //BT_CONVEX_SHAPE_INTERFACE1 +#endif //BT_CONVEX_SHAPE_INTERFACE1 diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index 0f9ced554b8..f6987cc760c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -19,42 +19,37 @@ subject to the following restrictions: #include "LinearMath/btQuaternion.h" #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" - -btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) -: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) +btConvexTriangleMeshShape ::btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb) + : btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) { m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; - if ( calcAabb ) + if (calcAabb) recalcLocalAabb(); } - - - ///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once ///but then we are duplicating -class LocalSupportVertexCallback: public btInternalTriangleIndexCallback +class LocalSupportVertexCallback : public btInternalTriangleIndexCallback { - btVector3 m_supportVertexLocal; -public: +public: btScalar m_maxDot; btVector3 m_supportVecLocal; LocalSupportVertexCallback(const btVector3& supportVecLocal) - : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), - m_maxDot(btScalar(-BT_LARGE_FLOAT)), - m_supportVecLocal(supportVecLocal) + : m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)), + m_maxDot(btScalar(-BT_LARGE_FLOAT)), + m_supportVecLocal(supportVecLocal) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { (void)triangleIndex; (void)partId; - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { btScalar dot = m_supportVecLocal.dot(triangle[i]); if (dot > m_maxDot) @@ -64,99 +59,82 @@ public: } } } - - btVector3 GetSupportVertexLocal() + + btVector3 GetSupportVertexLocal() { return m_supportVertexLocal; } - }; - - - - -btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 supVec(btScalar(0.), btScalar(0.), btScalar(0.)); btVector3 vec = vec0; btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } - LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + LocalSupportVertexCallback supportCallback(vec); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax); supVec = supportCallback.GetSupportVertexLocal(); return supVec; } -void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { //use 'w' component of supportVerticesOut? { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); } } - - ///@todo: could do the batch inside the callback! + ///@todo: could do the batch inside the callback! - for (int j=0;j<numVectors;j++) + for (int j = 0; j < numVectors; j++) { const btVector3& vec = vectors[j]; - LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + LocalSupportVertexCallback supportCallback(vec); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_stridingMesh->InternalProcessAllTriangles(&supportCallback, -aabbMax, aabbMax); supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); } - } - - -btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - if ( getMargin()!=btScalar(0.) ) + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - - - - - - - - //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo -int btConvexTriangleMeshShape::getNumVertices() const +int btConvexTriangleMeshShape::getNumVertices() const { //cache this? return 0; - } int btConvexTriangleMeshShape::getNumEdges() const @@ -164,44 +142,40 @@ int btConvexTriangleMeshShape::getNumEdges() const return 0; } -void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const +void btConvexTriangleMeshShape::getEdge(int, btVector3&, btVector3&) const { - btAssert(0); + btAssert(0); } -void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const +void btConvexTriangleMeshShape::getVertex(int, btVector3&) const { btAssert(0); } -int btConvexTriangleMeshShape::getNumPlanes() const +int btConvexTriangleMeshShape::getNumPlanes() const { return 0; } -void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const +void btConvexTriangleMeshShape::getPlane(btVector3&, btVector3&, int) const { btAssert(0); } //not yet -bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const +bool btConvexTriangleMeshShape::isInside(const btVector3&, btScalar) const { btAssert(0); return false; } - - -void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) +void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_stridingMesh->setScaling(scaling); - + recalcLocalAabb(); - } - const btVector3& btConvexTriangleMeshShape::getLocalScaling() const { return m_stridingMesh->getScaling(); @@ -209,107 +183,101 @@ const btVector3& btConvexTriangleMeshShape::getLocalScaling() const void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const { - class CenterCallback: public btInternalTriangleIndexCallback - { - bool first; - btVector3 ref; - btVector3 sum; - btScalar volume; - - public: - - CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) - { - (void) triangleIndex; - (void) partId; - if (first) - { - ref = triangle[0]; - first = false; - } - else - { - btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); - sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); - volume += vol; - } - } - - btVector3 getCenter() - { - return (volume > 0) ? sum / volume : ref; - } - - btScalar getVolume() - { - return volume * btScalar(1. / 6); - } - - }; - - class InertiaCallback: public btInternalTriangleIndexCallback - { - btMatrix3x3 sum; - btVector3 center; - - public: - - InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) - { - (void) triangleIndex; - (void) partId; - btMatrix3x3 i; - btVector3 a = triangle[0] - center; - btVector3 b = triangle[1] - center; - btVector3 c = triangle[2] - center; - btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); - for (int j = 0; j < 3; j++) - { - for (int k = 0; k <= j; k++) - { - i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) - + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j])); - } - } - btScalar i00 = -i[0][0]; - btScalar i11 = -i[1][1]; - btScalar i22 = -i[2][2]; - i[0][0] = i11 + i22; - i[1][1] = i22 + i00; - i[2][2] = i00 + i11; - sum[0] += i[0]; - sum[1] += i[1]; - sum[2] += i[2]; - } - - btMatrix3x3& getInertia() - { - return sum; - } - - }; - - CenterCallback centerCallback; - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); - btVector3 center = centerCallback.getCenter(); - principal.setOrigin(center); - volume = centerCallback.getVolume(); - - InertiaCallback inertiaCallback(center); - m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); - - btMatrix3x3& i = inertiaCallback.getInertia(); - i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); - inertia.setValue(i[0][0], i[1][1], i[2][2]); - inertia /= volume; -} + class CenterCallback : public btInternalTriangleIndexCallback + { + bool first; + btVector3 ref; + btVector3 sum; + btScalar volume; + + public: + CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + (void)triangleIndex; + (void)partId; + if (first) + { + ref = triangle[0]; + first = false; + } + else + { + btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); + sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); + volume += vol; + } + } + btVector3 getCenter() + { + return (volume > 0) ? sum / volume : ref; + } + + btScalar getVolume() + { + return volume * btScalar(1. / 6); + } + }; + + class InertiaCallback : public btInternalTriangleIndexCallback + { + btMatrix3x3 sum; + btVector3 center; + + public: + InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + (void)triangleIndex; + (void)partId; + btMatrix3x3 i; + btVector3 a = triangle[0] - center; + btVector3 b = triangle[1] - center; + btVector3 c = triangle[2] - center; + btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); + for (int j = 0; j < 3; j++) + { + for (int k = 0; k <= j; k++) + { + i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j])); + } + } + btScalar i00 = -i[0][0]; + btScalar i11 = -i[1][1]; + btScalar i22 = -i[2][2]; + i[0][0] = i11 + i22; + i[1][1] = i22 + i00; + i[2][2] = i00 + i11; + sum[0] += i[0]; + sum[1] += i[1]; + sum[2] += i[2]; + } + + btMatrix3x3& getInertia() + { + return sum; + } + }; + + CenterCallback centerCallback; + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); + btVector3 center = centerCallback.getCenter(); + principal.setOrigin(center); + volume = centerCallback.getVolume(); + + InertiaCallback inertiaCallback(center); + m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); + + btMatrix3x3& i = inertiaCallback.getInertia(); + i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); + inertia.setValue(i[0][0], i[1][1], i[2][2]); + inertia /= volume; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h index f338865ca13..6dac9fff040 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -15,24 +15,22 @@ subject to the following restrictions: #ifndef BT_CONVEX_TRIANGLEMESH_SHAPE_H #define BT_CONVEX_TRIANGLEMESH_SHAPE_H - #include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types - +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types /// The btConvexTriangleMeshShape is a convex hull of a triangle mesh, but the performance is not as good as btConvexHullShape. /// A small benefit of this class is that it uses the btStridingMeshInterface, so you can avoid the duplication of the triangle mesh data. Nevertheless, most users should use the much better performing btConvexHullShape instead. -ATTRIBUTE_ALIGNED16(class) btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btConvexTriangleMeshShape : public btPolyhedralConvexAabbCachingShape { - - class btStridingMeshInterface* m_stridingMesh; + class btStridingMeshInterface* m_stridingMesh; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb = true); - class btStridingMeshInterface* getMeshInterface() + btConvexTriangleMeshShape(btStridingMeshInterface * meshInterface, bool calcAabb = true); + + class btStridingMeshInterface* getMeshInterface() { return m_stridingMesh; } @@ -40,24 +38,23 @@ public: { return m_stridingMesh; } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + //debugging - virtual const char* getName()const {return "ConvexTrimesh";} - - virtual int getNumVertices() const; + virtual const char* getName() const { return "ConvexTrimesh"; } + + virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; + virtual void getVertex(int i, btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; - - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia @@ -65,13 +62,7 @@ public: ///by the mass. The resulting transform "principal" has to be applied inversely to the mesh in order for the local coordinate system of the ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform ///of the collision object by the principal transform. This method also computes the volume of the convex mesh. - void calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const; - + void calculatePrincipalAxisTransform(btTransform & principal, btVector3 & inertia, btScalar & volume) const; }; - - -#endif //BT_CONVEX_TRIANGLEMESH_SHAPE_H - - - +#endif //BT_CONVEX_TRIANGLEMESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 6cfe43be4da..66dbb8e53de 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -15,41 +15,37 @@ subject to the following restrictions: #include "btCylinderShape.h" -btCylinderShape::btCylinderShape (const btVector3& halfExtents) -:btConvexInternalShape(), -m_upAxis(1) +btCylinderShape::btCylinderShape(const btVector3& halfExtents) + : btConvexInternalShape(), + m_upAxis(1) { + btVector3 margin(getMargin(), getMargin(), getMargin()); + m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin; + setSafeMargin(halfExtents); - btVector3 margin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin; m_shapeType = CYLINDER_SHAPE_PROXYTYPE; } - -btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents) -:btCylinderShape(halfExtents) +btCylinderShapeX::btCylinderShapeX(const btVector3& halfExtents) + : btCylinderShape(halfExtents) { m_upAxis = 0; - } - -btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents) -:btCylinderShape(halfExtents) +btCylinderShapeZ::btCylinderShapeZ(const btVector3& halfExtents) + : btCylinderShape(halfExtents) { m_upAxis = 2; - } -void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btCylinderShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); + btTransformAabb(getHalfExtentsWithoutMargin(), getMargin(), t, aabbMin, aabbMax); } -void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btCylinderShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { - //Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility //#define USE_BOX_INERTIA_APPROXIMATION 1 #ifndef USE_BOX_INERTIA_APPROXIMATION @@ -63,25 +59,25 @@ void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co * */ - btScalar radius2; // square of cylinder radius - btScalar height2; // square of cylinder height - btVector3 halfExtents = getHalfExtentsWithMargin(); // get cylinder dimension + btScalar radius2; // square of cylinder radius + btScalar height2; // square of cylinder height + btVector3 halfExtents = getHalfExtentsWithMargin(); // get cylinder dimension btScalar div12 = mass / 12.f; btScalar div4 = mass / 4.f; btScalar div2 = mass / 2.f; int idxRadius, idxHeight; - switch (m_upAxis) // get indices of radius and height of cylinder + switch (m_upAxis) // get indices of radius and height of cylinder { - case 0: // cylinder is aligned along x + case 0: // cylinder is aligned along x idxRadius = 1; idxHeight = 0; break; - case 2: // cylinder is aligned along z + case 2: // cylinder is aligned along z idxRadius = 0; idxHeight = 2; break; - default: // cylinder is aligned along y + default: // cylinder is aligned along y idxRadius = 0; idxHeight = 1; } @@ -94,188 +90,164 @@ void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co btScalar t1 = div12 * height2 + div4 * radius2; btScalar t2 = div2 * radius2; - switch (m_upAxis) // set diagonal elements of inertia tensor + switch (m_upAxis) // set diagonal elements of inertia tensor { - case 0: // cylinder is aligned along x - inertia.setValue(t2,t1,t1); + case 0: // cylinder is aligned along x + inertia.setValue(t2, t1, t1); break; - case 2: // cylinder is aligned along z - inertia.setValue(t1,t1,t2); + case 2: // cylinder is aligned along z + inertia.setValue(t1, t1, t2); break; - default: // cylinder is aligned along y - inertia.setValue(t1,t2,t1); + default: // cylinder is aligned along y + inertia.setValue(t1, t2, t1); } -#else //USE_BOX_INERTIA_APPROXIMATION +#else //USE_BOX_INERTIA_APPROXIMATION //approximation of box shape btVector3 halfExtents = getHalfExtentsWithMargin(); - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); -#endif //USE_BOX_INERTIA_APPROXIMATION + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); +#endif //USE_BOX_INERTIA_APPROXIMATION } - -SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) +SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents, const btVector3& v) { -const int cylinderUpAxis = 0; -const int XX = 1; -const int YY = 0; -const int ZZ = 2; + const int cylinderUpAxis = 0; + const int XX = 1; + const int YY = 0; + const int ZZ = 2; //mapping depends on how cylinder local orientation is // extents of the cylinder is: X,Y is for radius, and Z for height - btScalar radius = halfExtents[XX]; btScalar halfHeight = halfExtents[cylinderUpAxis]; + btVector3 tmp; + btScalar d; - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) { - d = radius / s; + d = radius / s; tmp[XX] = v[XX] * d; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = v[ZZ] * d; return tmp; } - else + else { - tmp[XX] = radius; + tmp[XX] = radius; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = btScalar(0.0); return tmp; - } - - + } } - - - - - -inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v) +inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents, const btVector3& v) { - -const int cylinderUpAxis = 1; -const int XX = 0; -const int YY = 1; -const int ZZ = 2; - + const int cylinderUpAxis = 1; + const int XX = 0; + const int YY = 1; + const int ZZ = 2; btScalar radius = halfExtents[XX]; btScalar halfHeight = halfExtents[cylinderUpAxis]; + btVector3 tmp; + btScalar d; - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) { - d = radius / s; + d = radius / s; tmp[XX] = v[XX] * d; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = v[ZZ] * d; return tmp; } - else + else { - tmp[XX] = radius; + tmp[XX] = radius; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = btScalar(0.0); return tmp; - } - + } } -inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v) +inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents, const btVector3& v) { -const int cylinderUpAxis = 2; -const int XX = 0; -const int YY = 2; -const int ZZ = 1; + const int cylinderUpAxis = 2; + const int XX = 0; + const int YY = 2; + const int ZZ = 1; //mapping depends on how cylinder local orientation is // extents of the cylinder is: X,Y is for radius, and Z for height - btScalar radius = halfExtents[XX]; btScalar halfHeight = halfExtents[cylinderUpAxis]; + btVector3 tmp; + btScalar d; - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) { - d = radius / s; + d = radius / s; tmp[XX] = v[XX] * d; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = v[ZZ] * d; return tmp; } - else + else { - tmp[XX] = radius; + tmp[XX] = radius; tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; tmp[ZZ] = btScalar(0.0); return tmp; - } - - + } } -btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec); + return CylinderLocalSupportX(getHalfExtentsWithoutMargin(), vec); } - -btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec); + return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vec); } -btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec); + return CylinderLocalSupportY(getHalfExtentsWithoutMargin(), vec); } -void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { - supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]); + supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(), vectors[i]); } } -void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { - supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]); + supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vectors[i]); } } - - - -void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { - supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vectors[i]); + supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(), vectors[i]); } } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h index 6f796950e1d..d3f64508bae 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -17,90 +17,84 @@ subject to the following restrictions: #define BT_CYLINDER_MINKOWSKI_H #include "btBoxShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btVector3.h" /// The btCylinderShape class implements a cylinder shape primitive, centered around the origin. Its central axis aligned with the Y axis. btCylinderShapeX is aligned with the X axis and btCylinderShapeZ around the Z axis. -ATTRIBUTE_ALIGNED16(class) btCylinderShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btCylinderShape : public btConvexInternalShape { - protected: - - int m_upAxis; + int m_upAxis; public: - -BT_DECLARE_ALIGNED_ALLOCATOR(); + BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 getHalfExtentsWithMargin() const { btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); halfExtents += margin; return halfExtents; } - + const btVector3& getHalfExtentsWithoutMargin() const { - return m_implicitShapeDimensions;//changed in Bullet 2.63: assume the scaling and margin are included + return m_implicitShapeDimensions; //changed in Bullet 2.63: assume the scaling and margin are included } - btCylinderShape (const btVector3& halfExtents); - - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + btCylinderShape(const btVector3& halfExtents); + + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; virtual void setMargin(btScalar collisionMargin) { //correct the m_implicitShapeDimensions for the margin - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; - + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; + btConvexInternalShape::setMargin(collisionMargin); - btVector3 newMargin(getMargin(),getMargin(),getMargin()); + btVector3 newMargin(getMargin(), getMargin(), getMargin()); m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; - } - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { - btVector3 supVertex; supVertex = localGetSupportingVertexWithoutMargin(vec); - - if ( getMargin()!=btScalar(0.) ) + + if (getMargin() != btScalar(0.)) { btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; } return supVertex; } - //use box inertia // virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - int getUpAxis() const + int getUpAxis() const { return m_upAxis; } - virtual btVector3 getAnisotropicRollingFrictionDirection() const + virtual btVector3 getAnisotropicRollingFrictionDirection() const { - btVector3 aniDir(0,0,0); - aniDir[getUpAxis()]=1; + btVector3 aniDir(0, 0, 0); + aniDir[getUpAxis()] = 1; return aniDir; } @@ -109,43 +103,41 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); return getHalfExtentsWithMargin().getX(); } - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; + btVector3 oldMargin(getMargin(), getMargin(), getMargin()); + btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions + oldMargin; btVector3 unScaledImplicitShapeDimensionsWithMargin = implicitShapeDimensionsWithMargin / m_localScaling; btConvexInternalShape::setLocalScaling(scaling); m_implicitShapeDimensions = (unScaledImplicitShapeDimensionsWithMargin * m_localScaling) - oldMargin; - } //debugging - virtual const char* getName()const + virtual const char* getName() const { return "CylinderY"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; class btCylinderShapeX : public btCylinderShape { public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btCylinderShapeX (const btVector3& halfExtents); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - //debugging - virtual const char* getName()const + + btCylinderShapeX(const btVector3& halfExtents); + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + + //debugging + virtual const char* getName() const { return "CylinderX"; } @@ -154,21 +146,20 @@ public: { return getHalfExtentsWithMargin().getY(); } - }; class btCylinderShapeZ : public btCylinderShape { public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btCylinderShapeZ (const btVector3& halfExtents); - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + btCylinderShapeZ(const btVector3& halfExtents); - //debugging - virtual const char* getName()const + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + + //debugging + virtual const char* getName() const { return "CylinderZ"; } @@ -177,37 +168,39 @@ public: { return getHalfExtentsWithMargin().getX(); } - }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btCylinderShapeData +struct btCylinderShapeData { - btConvexInternalShapeData m_convexInternalShapeData; + btConvexInternalShapeData m_convexInternalShapeData; - int m_upAxis; + int m_upAxis; - char m_padding[4]; + char m_padding[4]; }; -SIMD_FORCE_INLINE int btCylinderShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btCylinderShape::calculateSerializeBufferSize() const { return sizeof(btCylinderShapeData); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btCylinderShapeData* shapeData = (btCylinderShapeData*) dataBuffer; - - btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); + btCylinderShapeData* shapeData = (btCylinderShapeData*)dataBuffer; - shapeData->m_upAxis = m_upAxis; - - return "btCylinderShapeData"; -} + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); + shapeData->m_upAxis = m_upAxis; + // Fill padding with zeros to appease msan. + shapeData->m_padding[0] = 0; + shapeData->m_padding[1] = 0; + shapeData->m_padding[2] = 0; + shapeData->m_padding[3] = 0; -#endif //BT_CYLINDER_MINKOWSKI_H + return "btCylinderShapeData"; +} +#endif //BT_CYLINDER_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp index a9e6df5c58e..4699555bd87 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp @@ -15,36 +15,28 @@ subject to the following restrictions: #include "btEmptyShape.h" - #include "btCollisionShape.h" - -btEmptyShape::btEmptyShape() : btConcaveShape () +btEmptyShape::btEmptyShape() : btConcaveShape() { m_shapeType = EMPTY_SHAPE_PROXYTYPE; } - btEmptyShape::~btEmptyShape() { } - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btEmptyShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 margin(getMargin(),getMargin(),getMargin()); + btVector3 margin(getMargin(), getMargin(), getMargin()); aabbMin = t.getOrigin() - margin; aabbMax = t.getOrigin() + margin; - } -void btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) const +void btEmptyShape::calculateLocalInertia(btScalar, btVector3&) const { btAssert(0); } - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h index 069a79402bf..d2e21173b21 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h @@ -23,50 +23,43 @@ subject to the following restrictions: #include "LinearMath/btMatrix3x3.h" #include "btCollisionMargin.h" - - - /// The btEmptyShape is a collision shape without actual collision detection shape, so most users should ignore this class. /// It can be replaced by another shape during runtime, but the inertia tensor should be recomputed. -ATTRIBUTE_ALIGNED16(class) btEmptyShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btEmptyShape : public btConcaveShape { public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btEmptyShape(); virtual ~btEmptyShape(); - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - - virtual void setLocalScaling(const btVector3& scaling) + virtual void setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } - virtual const btVector3& getLocalScaling() const + virtual const btVector3& getLocalScaling() const { return m_localScaling; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual const char* getName()const + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; + + virtual const char* getName() const { return "Empty"; } - virtual void processAllTriangles(btTriangleCallback* ,const btVector3& ,const btVector3& ) const + virtual void processAllTriangles(btTriangleCallback*, const btVector3&, const btVector3&) const { } protected: - btVector3 m_localScaling; - + btVector3 m_localScaling; }; - - -#endif //BT_EMPTY_SHAPE_H +#endif //BT_EMPTY_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 441a89c6bb6..34e7926f17c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -17,23 +17,23 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" - - -btHeightfieldTerrainShape::btHeightfieldTerrainShape -( -int heightStickWidth, int heightStickLength, const void* heightfieldData, -btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis, -PHY_ScalarType hdt, bool flipQuadEdges -) +btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, const void* heightfieldData, + btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType hdt, bool flipQuadEdges) + :m_userIndex2(-1), + m_userValue3(0), + m_triangleInfoMap(0) { initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); } - - -btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,const void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) +btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges) + :m_userIndex2(-1), + m_userValue3(0), + m_triangleInfoMap(0) { // legacy constructor: support only float or unsigned char, // and min height is zero @@ -45,27 +45,23 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int h btScalar heightScale = maxHeight / 65535; initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); } - - -void btHeightfieldTerrainShape::initialize -( -int heightStickWidth, int heightStickLength, const void* heightfieldData, -btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, -PHY_ScalarType hdt, bool flipQuadEdges -) +void btHeightfieldTerrainShape::initialize( + int heightStickWidth, int heightStickLength, const void* heightfieldData, + btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType hdt, bool flipQuadEdges) { // validation - btAssert(heightStickWidth > 1);// && "bad width"); - btAssert(heightStickLength > 1);// && "bad length"); - btAssert(heightfieldData);// && "null heightfield data"); + btAssert(heightStickWidth > 1); // && "bad width"); + btAssert(heightStickLength > 1); // && "bad length"); + btAssert(heightfieldData); // && "null heightfield data"); // btAssert(heightScale) -- do we care? Trust caller here - btAssert(minHeight <= maxHeight);// && "bad min/max height"); - btAssert(upAxis >= 0 && upAxis < 3);// && "bad upAxis--should be in range [0,2]"); - btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT);// && "Bad height data type enum"); + btAssert(minHeight <= maxHeight); // && "bad min/max height"); + btAssert(upAxis >= 0 && upAxis < 3); // && "bad upAxis--should be in range [0,2]"); + btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT); // && "Bad height data type enum"); // initialize member variables m_shapeType = TERRAIN_SHAPE_PROXYTYPE; @@ -73,42 +69,47 @@ PHY_ScalarType hdt, bool flipQuadEdges m_heightStickLength = heightStickLength; m_minHeight = minHeight; m_maxHeight = maxHeight; - m_width = (btScalar) (heightStickWidth - 1); - m_length = (btScalar) (heightStickLength - 1); + m_width = (btScalar)(heightStickWidth - 1); + m_length = (btScalar)(heightStickLength - 1); m_heightScale = heightScale; m_heightfieldDataUnknown = heightfieldData; m_heightDataType = hdt; m_flipQuadEdges = flipQuadEdges; m_useDiamondSubdivision = false; m_useZigzagSubdivision = false; + m_flipTriangleWinding = false; m_upAxis = upAxis; m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); + + m_vboundsChunkSize = 0; + m_vboundsGridWidth = 0; + m_vboundsGridLength = 0; // determine min/max axis-aligned bounding box (aabb) values switch (m_upAxis) { - case 0: + case 0: { m_localAabbMin.setValue(m_minHeight, 0, 0); m_localAabbMax.setValue(m_maxHeight, m_width, m_length); break; } - case 1: + case 1: { m_localAabbMin.setValue(0, m_minHeight, 0); m_localAabbMax.setValue(m_width, m_maxHeight, m_length); break; }; - case 2: + case 2: { m_localAabbMin.setValue(0, 0, m_minHeight); m_localAabbMax.setValue(m_width, m_length, m_maxHeight); break; } - default: + default: { //need to get valid m_upAxis - btAssert(0);// && "Bad m_upAxis"); + btAssert(0); // && "Bad m_upAxis"); } } @@ -116,62 +117,58 @@ PHY_ScalarType hdt, bool flipQuadEdges m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); } - - btHeightfieldTerrainShape::~btHeightfieldTerrainShape() { + clearAccelerator(); } - - -void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { - btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); + btVector3 halfExtents = (m_localAabbMax - m_localAabbMin) * m_localScaling * btScalar(0.5); btVector3 localOrigin(0, 0, 0); localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); localOrigin *= m_localScaling; - btMatrix3x3 abs_b = t.getBasis().absolute(); + btMatrix3x3 abs_b = t.getBasis().absolute(); btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - extent += btVector3(getMargin(),getMargin(),getMargin()); + btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + extent += btVector3(getMargin(), getMargin(), getMargin()); aabbMin = center - extent; aabbMax = center + extent; } - /// This returns the "raw" (user's initial) height, not the actual height. /// The actual height needs to be adjusted to be relative to the center /// of the heightfield's AABB. btScalar -btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const +btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const { btScalar val = 0.f; switch (m_heightDataType) { - case PHY_FLOAT: + case PHY_FLOAT: { - val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; + val = m_heightfieldDataFloat[(y * m_heightStickWidth) + x]; break; } - case PHY_UCHAR: + case PHY_UCHAR: { - unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; + unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x]; val = heightFieldValue * m_heightScale; break; } - case PHY_SHORT: + case PHY_SHORT: { short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; val = hfValue * m_heightScale; break; } - default: + default: { btAssert(!"Bad m_heightDataType"); } @@ -180,74 +177,63 @@ btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const return val; } - - - /// this returns the vertex in bullet-local coordinates -void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const +void btHeightfieldTerrainShape::getVertex(int x, int y, btVector3& vertex) const { - btAssert(x>=0); - btAssert(y>=0); - btAssert(x<m_heightStickWidth); - btAssert(y<m_heightStickLength); + btAssert(x >= 0); + btAssert(y >= 0); + btAssert(x < m_heightStickWidth); + btAssert(y < m_heightStickLength); - btScalar height = getRawHeightFieldValue(x,y); + btScalar height = getRawHeightFieldValue(x, y); switch (m_upAxis) { - case 0: + case 0: { - vertex.setValue( - height - m_localOrigin.getX(), - (-m_width/btScalar(2.0)) + x, - (-m_length/btScalar(2.0) ) + y - ); + vertex.setValue( + height - m_localOrigin.getX(), + (-m_width / btScalar(2.0)) + x, + (-m_length / btScalar(2.0)) + y); break; } - case 1: + case 1: { vertex.setValue( - (-m_width/btScalar(2.0)) + x, - height - m_localOrigin.getY(), - (-m_length/btScalar(2.0)) + y - ); + (-m_width / btScalar(2.0)) + x, + height - m_localOrigin.getY(), + (-m_length / btScalar(2.0)) + y); break; }; - case 2: + case 2: { vertex.setValue( - (-m_width/btScalar(2.0)) + x, - (-m_length/btScalar(2.0)) + y, - height - m_localOrigin.getZ() - ); + (-m_width / btScalar(2.0)) + x, + (-m_length / btScalar(2.0)) + y, + height - m_localOrigin.getZ()); break; } - default: + default: { //need to get valid m_upAxis btAssert(0); } } - vertex*=m_localScaling; + vertex *= m_localScaling; } - - static inline int -getQuantized -( -btScalar x -) +getQuantized( + btScalar x) { - if (x < 0.0) { - return (int) (x - 0.5); + if (x < 0.0) + { + return (int)(x - 0.5); } - return (int) (x + 0.5); + return (int)(x + 0.5); } - - /// given input vector, return quantized version /** This routine is basically determining the gridpoint indices for a given @@ -257,7 +243,7 @@ btScalar x "with clamp" means that we restrict the point to be in the heightfield's axis-aligned bounding box. */ -void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const +void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point, int /*isMax*/) const { btVector3 clampedPoint(point); clampedPoint.setMax(m_localAabbMin); @@ -266,11 +252,8 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi out[0] = getQuantized(clampedPoint.getX()); out[1] = getQuantized(clampedPoint.getY()); out[2] = getQuantized(clampedPoint.getZ()); - } - - /// process all triangles within the provided axis-aligned bounding box /** basic algorithm: @@ -278,128 +261,132 @@ void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& poi - convert input aabb to a range of heightfield grid points (quantize) - iterate over all triangles in that subset of the grid */ -void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { // scale down the input aabb's so they are in local (non-scaled) coordinates - btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); - btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); + btVector3 localAabbMin = aabbMin * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]); + btVector3 localAabbMax = aabbMax * btVector3(1.f / m_localScaling[0], 1.f / m_localScaling[1], 1.f / m_localScaling[2]); // account for local origin localAabbMin += m_localOrigin; localAabbMax += m_localOrigin; //quantize the aabbMin and aabbMax, and adjust the start/end ranges - int quantizedAabbMin[3]; - int quantizedAabbMax[3]; - quantizeWithClamp(quantizedAabbMin, localAabbMin,0); - quantizeWithClamp(quantizedAabbMax, localAabbMax,1); - + int quantizedAabbMin[3]; + int quantizedAabbMax[3]; + quantizeWithClamp(quantizedAabbMin, localAabbMin, 0); + quantizeWithClamp(quantizedAabbMax, localAabbMax, 1); + // expand the min/max quantized values // this is to catch the case where the input aabb falls between grid points! - for (int i = 0; i < 3; ++i) { + for (int i = 0; i < 3; ++i) + { quantizedAabbMin[i]--; quantizedAabbMax[i]++; - } + } - int startX=0; - int endX=m_heightStickWidth-1; - int startJ=0; - int endJ=m_heightStickLength-1; + int startX = 0; + int endX = m_heightStickWidth - 1; + int startJ = 0; + int endJ = m_heightStickLength - 1; switch (m_upAxis) { - case 0: + case 0: { - if (quantizedAabbMin[1]>startX) + if (quantizedAabbMin[1] > startX) startX = quantizedAabbMin[1]; - if (quantizedAabbMax[1]<endX) + if (quantizedAabbMax[1] < endX) endX = quantizedAabbMax[1]; - if (quantizedAabbMin[2]>startJ) + if (quantizedAabbMin[2] > startJ) startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]<endJ) + if (quantizedAabbMax[2] < endJ) endJ = quantizedAabbMax[2]; break; } - case 1: + case 1: { - if (quantizedAabbMin[0]>startX) + if (quantizedAabbMin[0] > startX) startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]<endX) + if (quantizedAabbMax[0] < endX) endX = quantizedAabbMax[0]; - if (quantizedAabbMin[2]>startJ) + if (quantizedAabbMin[2] > startJ) startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]<endJ) + if (quantizedAabbMax[2] < endJ) endJ = quantizedAabbMax[2]; break; }; - case 2: + case 2: { - if (quantizedAabbMin[0]>startX) + if (quantizedAabbMin[0] > startX) startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]<endX) + if (quantizedAabbMax[0] < endX) endX = quantizedAabbMax[0]; - if (quantizedAabbMin[1]>startJ) + if (quantizedAabbMin[1] > startJ) startJ = quantizedAabbMin[1]; - if (quantizedAabbMax[1]<endJ) + if (quantizedAabbMax[1] < endJ) endJ = quantizedAabbMax[1]; break; } - default: + default: { //need to get valid m_upAxis btAssert(0); } } - - + // TODO If m_vboundsGrid is available, use it to determine if we really need to process this area - for(int j=startJ; j<endJ; j++) + for (int j = startJ; j < endJ; j++) { - for(int x=startX; x<endX; x++) + for (int x = startX; x < endX; x++) { btVector3 vertices[3]; - if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))|| (m_useZigzagSubdivision && !(j & 1))) + int indices[3] = { 0, 1, 2 }; + if (m_flipTriangleWinding) + { + indices[0] = 2; + indices[2] = 0; + } + + if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1))) { - //first triangle - getVertex(x,j,vertices[0]); - getVertex(x, j + 1, vertices[1]); - getVertex(x + 1, j + 1, vertices[2]); - callback->processTriangle(vertices,x,j); - //second triangle - // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman - getVertex(x+1,j+1,vertices[1]); - getVertex(x + 1, j, vertices[2]); - callback->processTriangle(vertices, x, j); - - } else + //first triangle + getVertex(x, j, vertices[indices[0]]); + getVertex(x, j + 1, vertices[indices[1]]); + getVertex(x + 1, j + 1, vertices[indices[2]]); + callback->processTriangle(vertices, 2 * x, j); + //second triangle + // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman + getVertex(x + 1, j + 1, vertices[indices[1]]); + getVertex(x + 1, j, vertices[indices[2]]); + callback->processTriangle(vertices, 2 * x+1, j); + } + else { - //first triangle - getVertex(x,j,vertices[0]); - getVertex(x,j+1,vertices[1]); - getVertex(x+1,j,vertices[2]); - callback->processTriangle(vertices,x,j); - //second triangle - getVertex(x+1,j,vertices[0]); - //getVertex(x,j+1,vertices[1]); - getVertex(x+1,j+1,vertices[2]); - callback->processTriangle(vertices,x,j); + //first triangle + getVertex(x, j, vertices[indices[0]]); + getVertex(x, j + 1, vertices[indices[1]]); + getVertex(x + 1, j, vertices[indices[2]]); + callback->processTriangle(vertices, 2 * x, j); + //second triangle + getVertex(x + 1, j, vertices[indices[0]]); + //getVertex(x,j+1,vertices[1]); + getVertex(x + 1, j + 1, vertices[indices[2]]); + callback->processTriangle(vertices, 2 * x+1, j); } } } - - - } -void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const +void btHeightfieldTerrainShape::calculateLocalInertia(btScalar, btVector3& inertia) const { //moving concave objects not supported - - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } -void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) +void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } @@ -407,3 +394,458 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const { return m_localScaling; } + +namespace +{ + struct GridRaycastState + { + int x; // Next quad coords + int z; + int prev_x; // Previous quad coords + int prev_z; + btScalar param; // Exit param for previous quad + btScalar prevParam; // Enter param for previous quad + btScalar maxDistanceFlat; + btScalar maxDistance3d; + }; +} + +// TODO Does it really need to take 3D vectors? +/// Iterates through a virtual 2D grid of unit-sized square cells, +/// and executes an action on each cell intersecting the given segment, ordered from begin to end. +/// Initially inspired by http://www.cse.yorku.ca/~amana/research/grid.pdf +template <typename Action_T> +void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector3& endPos, int indices[3]) +{ + GridRaycastState rs; + rs.maxDistance3d = beginPos.distance(endPos); + if (rs.maxDistance3d < 0.0001) + { + // Consider the ray is too small to hit anything + return; + } + + + btScalar rayDirectionFlatX = endPos[indices[0]] - beginPos[indices[0]]; + btScalar rayDirectionFlatZ = endPos[indices[2]] - beginPos[indices[2]]; + rs.maxDistanceFlat = btSqrt(rayDirectionFlatX * rayDirectionFlatX + rayDirectionFlatZ * rayDirectionFlatZ); + + if (rs.maxDistanceFlat < 0.0001) + { + // Consider the ray vertical + rayDirectionFlatX = 0; + rayDirectionFlatZ = 0; + } + else + { + rayDirectionFlatX /= rs.maxDistanceFlat; + rayDirectionFlatZ /= rs.maxDistanceFlat; + } + + const int xiStep = rayDirectionFlatX > 0 ? 1 : rayDirectionFlatX < 0 ? -1 : 0; + const int ziStep = rayDirectionFlatZ > 0 ? 1 : rayDirectionFlatZ < 0 ? -1 : 0; + + const float infinite = 9999999; + const btScalar paramDeltaX = xiStep != 0 ? 1.f / btFabs(rayDirectionFlatX) : infinite; + const btScalar paramDeltaZ = ziStep != 0 ? 1.f / btFabs(rayDirectionFlatZ) : infinite; + + // pos = param * dir + btScalar paramCrossX; // At which value of `param` we will cross a x-axis lane? + btScalar paramCrossZ; // At which value of `param` we will cross a z-axis lane? + + // paramCrossX and paramCrossZ are initialized as being the first cross + // X initialization + if (xiStep != 0) + { + if (xiStep == 1) + { + paramCrossX = (ceil(beginPos[indices[0]]) - beginPos[indices[0]]) * paramDeltaX; + } + else + { + paramCrossX = (beginPos[indices[0]] - floor(beginPos[indices[0]])) * paramDeltaX; + } + } + else + { + paramCrossX = infinite; // Will never cross on X + } + + // Z initialization + if (ziStep != 0) + { + if (ziStep == 1) + { + paramCrossZ = (ceil(beginPos[indices[2]]) - beginPos[indices[2]]) * paramDeltaZ; + } + else + { + paramCrossZ = (beginPos[indices[2]] - floor(beginPos[indices[2]])) * paramDeltaZ; + } + } + else + { + paramCrossZ = infinite; // Will never cross on Z + } + + rs.x = static_cast<int>(floor(beginPos[indices[0]])); + rs.z = static_cast<int>(floor(beginPos[indices[2]])); + + // Workaround cases where the ray starts at an integer position + if (paramCrossX == 0.0) + { + paramCrossX += paramDeltaX; + // If going backwards, we should ignore the position we would get by the above flooring, + // because the ray is not heading in that direction + if (xiStep == -1) + { + rs.x -= 1; + } + } + + if (paramCrossZ == 0.0) + { + paramCrossZ += paramDeltaZ; + if (ziStep == -1) + rs.z -= 1; + } + + rs.prev_x = rs.x; + rs.prev_z = rs.z; + rs.param = 0; + + while (true) + { + rs.prev_x = rs.x; + rs.prev_z = rs.z; + rs.prevParam = rs.param; + + if (paramCrossX < paramCrossZ) + { + // X lane + rs.x += xiStep; + // Assign before advancing the param, + // to be in sync with the initialization step + rs.param = paramCrossX; + paramCrossX += paramDeltaX; + } + else + { + // Z lane + rs.z += ziStep; + rs.param = paramCrossZ; + paramCrossZ += paramDeltaZ; + } + + if (rs.param > rs.maxDistanceFlat) + { + rs.param = rs.maxDistanceFlat; + quadAction(rs); + break; + } + else + { + quadAction(rs); + } + } +} + +struct ProcessTrianglesAction +{ + const btHeightfieldTerrainShape* shape; + bool flipQuadEdges; + bool useDiamondSubdivision; + int width; + int length; + btTriangleCallback* callback; + + void exec(int x, int z) const + { + if (x < 0 || z < 0 || x >= width || z >= length) + { + return; + } + + btVector3 vertices[3]; + + // TODO Since this is for raycasts, we could greatly benefit from an early exit on the first hit + + // Check quad + if (flipQuadEdges || (useDiamondSubdivision && (((z + x) & 1) > 0))) + { + // First triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x + 1, z, vertices[1]); + shape->getVertex(x + 1, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + + // Second triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x + 1, z + 1, vertices[1]); + shape->getVertex(x, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + } + else + { + // First triangle + shape->getVertex(x, z, vertices[0]); + shape->getVertex(x, z + 1, vertices[1]); + shape->getVertex(x + 1, z, vertices[2]); + callback->processTriangle(vertices, x, z); + + // Second triangle + shape->getVertex(x + 1, z, vertices[0]); + shape->getVertex(x, z + 1, vertices[1]); + shape->getVertex(x + 1, z + 1, vertices[2]); + callback->processTriangle(vertices, x, z); + } + } + + void operator()(const GridRaycastState& bs) const + { + exec(bs.prev_x, bs.prev_z); + } +}; + +struct ProcessVBoundsAction +{ + const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& vbounds; + int width; + int length; + int chunkSize; + + btVector3 rayBegin; + btVector3 rayEnd; + btVector3 rayDir; + + int* m_indices; + ProcessTrianglesAction processTriangles; + + ProcessVBoundsAction(const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& bnd, int* indices) + : vbounds(bnd), + m_indices(indices) + { + } + void operator()(const GridRaycastState& rs) const + { + int x = rs.prev_x; + int z = rs.prev_z; + + if (x < 0 || z < 0 || x >= width || z >= length) + { + return; + } + + const btHeightfieldTerrainShape::Range chunk = vbounds[x + z * width]; + + btVector3 enterPos; + btVector3 exitPos; + + if (rs.maxDistanceFlat > 0.0001) + { + btScalar flatTo3d = chunkSize * rs.maxDistance3d / rs.maxDistanceFlat; + btScalar enterParam3d = rs.prevParam * flatTo3d; + btScalar exitParam3d = rs.param * flatTo3d; + enterPos = rayBegin + rayDir * enterParam3d; + exitPos = rayBegin + rayDir * exitParam3d; + + // We did enter the flat projection of the AABB, + // but we have to check if we intersect it on the vertical axis + if (enterPos[1] > chunk.max && exitPos[m_indices[1]] > chunk.max) + { + return; + } + if (enterPos[1] < chunk.min && exitPos[m_indices[1]] < chunk.min) + { + return; + } + } + else + { + // Consider the ray vertical + // (though we shouldn't reach this often because there is an early check up-front) + enterPos = rayBegin; + exitPos = rayEnd; + } + + gridRaycast(processTriangles, enterPos, exitPos, m_indices); + // Note: it could be possible to have more than one grid at different levels, + // to do this there would be a branch using a pointer to another ProcessVBoundsAction + } +}; + +// TODO How do I interrupt the ray when there is a hit? `callback` does not return any result +/// Performs a raycast using a hierarchical Bresenham algorithm. +/// Does not allocate any memory by itself. +void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) const +{ + // Transform to cell-local + btVector3 beginPos = raySource / m_localScaling; + btVector3 endPos = rayTarget / m_localScaling; + beginPos += m_localOrigin; + endPos += m_localOrigin; + + ProcessTrianglesAction processTriangles; + processTriangles.shape = this; + processTriangles.flipQuadEdges = m_flipQuadEdges; + processTriangles.useDiamondSubdivision = m_useDiamondSubdivision; + processTriangles.callback = callback; + processTriangles.width = m_heightStickWidth - 1; + processTriangles.length = m_heightStickLength - 1; + + // TODO Transform vectors to account for m_upAxis + int indices[3] = { 0, 1, 2 }; + if (m_upAxis == 2) + { + indices[1] = 2; + indices[2] = 1; + } + int iBeginX = static_cast<int>(floor(beginPos[indices[0]])); + int iBeginZ = static_cast<int>(floor(beginPos[indices[2]])); + int iEndX = static_cast<int>(floor(endPos[indices[0]])); + int iEndZ = static_cast<int>(floor(endPos[indices[2]])); + + if (iBeginX == iEndX && iBeginZ == iEndZ) + { + // The ray will never cross quads within the plane, + // so directly process triangles within one quad + // (typically, vertical rays should end up here) + processTriangles.exec(iBeginX, iEndZ); + return; + } + + + + if (m_vboundsGrid.size()==0) + { + // Process all quads intersecting the flat projection of the ray + gridRaycast(processTriangles, beginPos, endPos, &indices[0]); + } + else + { + btVector3 rayDiff = endPos - beginPos; + btScalar flatDistance2 = rayDiff[indices[0]] * rayDiff[indices[0]] + rayDiff[indices[2]] * rayDiff[indices[2]]; + if (flatDistance2 < m_vboundsChunkSize * m_vboundsChunkSize) + { + // Don't use chunks, the ray is too short in the plane + gridRaycast(processTriangles, beginPos, endPos, &indices[0]); + } + + ProcessVBoundsAction processVBounds(m_vboundsGrid, &indices[0]); + processVBounds.width = m_vboundsGridWidth; + processVBounds.length = m_vboundsGridLength; + processVBounds.rayBegin = beginPos; + processVBounds.rayEnd = endPos; + processVBounds.rayDir = rayDiff.normalized(); + processVBounds.processTriangles = processTriangles; + processVBounds.chunkSize = m_vboundsChunkSize; + // The ray is long, run raycast on a higher-level grid + gridRaycast(processVBounds, beginPos / m_vboundsChunkSize, endPos / m_vboundsChunkSize, indices); + } +} + +/// Builds a grid data structure storing the min and max heights of the terrain in chunks. +/// if chunkSize is zero, that accelerator is removed. +/// If you modify the heights, you need to rebuild this accelerator. +void btHeightfieldTerrainShape::buildAccelerator(int chunkSize) +{ + if (chunkSize <= 0) + { + clearAccelerator(); + return; + } + + m_vboundsChunkSize = chunkSize; + int nChunksX = m_heightStickWidth / chunkSize; + int nChunksZ = m_heightStickLength / chunkSize; + + if (m_heightStickWidth % chunkSize > 0) + { + ++nChunksX; // In case terrain size isn't dividable by chunk size + } + if (m_heightStickLength % chunkSize > 0) + { + ++nChunksZ; + } + + if (m_vboundsGridWidth != nChunksX || m_vboundsGridLength != nChunksZ) + { + clearAccelerator(); + m_vboundsGridWidth = nChunksX; + m_vboundsGridLength = nChunksZ; + } + + if (nChunksX == 0 || nChunksZ == 0) + { + return; + } + + // This data structure is only reallocated if the required size changed + m_vboundsGrid.resize(nChunksX * nChunksZ); + + // Compute min and max height for all chunks + for (int cz = 0; cz < nChunksZ; ++cz) + { + int z0 = cz * chunkSize; + + for (int cx = 0; cx < nChunksX; ++cx) + { + int x0 = cx * chunkSize; + + Range r; + + r.min = getRawHeightFieldValue(x0, z0); + r.max = r.min; + + // Compute min and max height for this chunk. + // We have to include one extra cell to account for neighbors. + // Here is why: + // Say we have a flat terrain, and a plateau that fits a chunk perfectly. + // + // Left Right + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // | | | | | | + // 0---0---0---1---1---1 + // x + // + // If the AABB for the Left chunk did not share vertices with the Right, + // then we would fail collision tests at x due to a gap. + // + for (int z = z0; z < z0 + chunkSize + 1; ++z) + { + if (z >= m_heightStickLength) + { + continue; + } + + for (int x = x0; x < x0 + chunkSize + 1; ++x) + { + if (x >= m_heightStickWidth) + { + continue; + } + + btScalar height = getRawHeightFieldValue(x, z); + + if (height < r.min) + { + r.min = height; + } + else if (height > r.max) + { + r.max = height; + } + } + } + + m_vboundsGrid[cx + cz * nChunksX] = r; + } + } +} + +void btHeightfieldTerrainShape::clearAccelerator() +{ + m_vboundsGrid.clear(); +}
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 4a7a4a4bda4..8dea98fc6be 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -17,6 +17,7 @@ subject to the following restrictions: #define BT_HEIGHTFIELD_TERRAIN_SHAPE_H #include "btConcaveShape.h" +#include "LinearMath/btAlignedObjectArray.h" ///btHeightfieldTerrainShape simulates a 2D heightfield terrain /** @@ -68,43 +69,58 @@ subject to the following restrictions: For usage and testing see the TerrainDemo. */ -ATTRIBUTE_ALIGNED16(class) btHeightfieldTerrainShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btHeightfieldTerrainShape : public btConcaveShape { +public: + struct Range + { + btScalar min; + btScalar max; + }; + protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - btVector3 m_localOrigin; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + btVector3 m_localOrigin; ///terrain data - int m_heightStickWidth; + int m_heightStickWidth; int m_heightStickLength; - btScalar m_minHeight; - btScalar m_maxHeight; + btScalar m_minHeight; + btScalar m_maxHeight; btScalar m_width; btScalar m_length; btScalar m_heightScale; - union - { - const unsigned char* m_heightfieldDataUnsignedChar; - const short* m_heightfieldDataShort; - const btScalar* m_heightfieldDataFloat; - const void* m_heightfieldDataUnknown; + union { + const unsigned char* m_heightfieldDataUnsignedChar; + const short* m_heightfieldDataShort; + const btScalar* m_heightfieldDataFloat; + const void* m_heightfieldDataUnknown; }; - PHY_ScalarType m_heightDataType; - bool m_flipQuadEdges; - bool m_useDiamondSubdivision; + PHY_ScalarType m_heightDataType; + bool m_flipQuadEdges; + bool m_useDiamondSubdivision; bool m_useZigzagSubdivision; + bool m_flipTriangleWinding; + int m_upAxis; - int m_upAxis; - - btVector3 m_localScaling; + btVector3 m_localScaling; + + // Accelerator + btAlignedObjectArray<Range> m_vboundsGrid; + int m_vboundsGridWidth; + int m_vboundsGridLength; + int m_vboundsChunkSize; - virtual btScalar getRawHeightFieldValue(int x,int y) const; - void quantizeWithClamp(int* out, const btVector3& point,int isMax) const; - void getVertex(int x,int y,btVector3& vertex) const; + int m_userIndex2; + btScalar m_userValue3; + struct btTriangleInfoMap* m_triangleInfoMap; + virtual btScalar getRawHeightFieldValue(int x, int y) const; + void quantizeWithClamp(int* out, const btVector3& point, int isMax) const; /// protected initialization /** @@ -112,25 +128,24 @@ protected: backwards-compatible without a lot of copy/paste. */ void initialize(int heightStickWidth, int heightStickLength, - const void* heightfieldData, btScalar heightScale, - btScalar minHeight, btScalar maxHeight, int upAxis, - PHY_ScalarType heightDataType, bool flipQuadEdges); + const void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType heightDataType, bool flipQuadEdges); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - + /// preferred constructor /** This constructor supports a range of heightfield data types, and allows for a non-zero minimum height value. heightScale is needed for any integer-based heightfield data types. */ - btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength, - const void* heightfieldData, btScalar heightScale, - btScalar minHeight, btScalar maxHeight, - int upAxis, PHY_ScalarType heightDataType, - bool flipQuadEdges); + btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, + const void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, + int upAxis, PHY_ScalarType heightDataType, + bool flipQuadEdges); /// legacy constructor /** @@ -139,29 +154,76 @@ public: compatibility reasons, heightScale is calculated as maxHeight / 65535 (and is only used when useFloatData = false). */ - btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,const void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); + btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges); virtual ~btHeightfieldTerrainShape(); + void setUseDiamondSubdivision(bool useDiamondSubdivision = true) { m_useDiamondSubdivision = useDiamondSubdivision; } - void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} + ///could help compatibility with Ogre heightfields. See https://code.google.com/p/bullet/issues/detail?id=625 + void setUseZigzagSubdivision(bool useZigzagSubdivision = true) { m_useZigzagSubdivision = useZigzagSubdivision; } - ///could help compatibility with Ogre heightfields. See https://code.google.com/p/bullet/issues/detail?id=625 - void setUseZigzagSubdivision(bool useZigzagSubdivision=true) { m_useZigzagSubdivision = useZigzagSubdivision;} + void setFlipTriangleWinding(bool flipTriangleWinding) + { + m_flipTriangleWinding = flipTriangleWinding; + } + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void setLocalScaling(const btVector3& scaling); - virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const; - + + void getVertex(int x, int y, btVector3& vertex) const; + + void performRaycast(btTriangleCallback * callback, const btVector3& raySource, const btVector3& rayTarget) const; + + void buildAccelerator(int chunkSize = 16); + void clearAccelerator(); + + int getUpAxis() const + { + return m_upAxis; + } //debugging - virtual const char* getName()const {return "HEIGHTFIELD";} + virtual const char* getName() const { return "HEIGHTFIELD"; } + + void setUserIndex2(int index) + { + m_userIndex2 = index; + } + int getUserIndex2() const + { + return m_userIndex2; + } + void setUserValue3(btScalar value) + { + m_userValue3 = value; + } + btScalar getUserValue3() const + { + return m_userValue3; + } + const struct btTriangleInfoMap* getTriangleInfoMap() const + { + return m_triangleInfoMap; + } + struct btTriangleInfoMap* getTriangleInfoMap() + { + return m_triangleInfoMap; + } + void setTriangleInfoMap(btTriangleInfoMap* map) + { + m_triangleInfoMap = map; + } + const unsigned char* getHeightfieldRawData() const + { + return m_heightfieldDataUnsignedChar; + } }; -#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H +#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h index 866f9b4da4d..c9a436bf26a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h @@ -21,15 +21,18 @@ subject to the following restrictions: // Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties class btMaterial { - // public members so that materials can change due to world events + // public members so that materials can change due to world events public: - btScalar m_friction; - btScalar m_restitution; - int pad[2]; - - btMaterial(){} - btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; } + btScalar m_friction; + btScalar m_restitution; + int pad[2]; + + btMaterial() {} + btMaterial(btScalar fric, btScalar rest) + { + m_friction = fric; + m_restitution = rest; + } }; -#endif // BT_MATERIAL_H - +#endif // BT_MATERIAL_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.cpp new file mode 100644 index 00000000000..13c0a343f17 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.cpp @@ -0,0 +1,522 @@ +#include "btMiniSDF.h" + +// +//Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid +//example: +//GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj +//The MIT License (MIT) +// +//Copyright (c) 2017 Dan Koschier +// + +#include <limits.h> +#include <string.h> //memcpy + +struct btSdfDataStream +{ + const char* m_data; + int m_size; + + int m_currentOffset; + + btSdfDataStream(const char* data, int size) + : m_data(data), + m_size(size), + m_currentOffset(0) + { + } + + template <class T> + bool read(T& val) + { + int bytes = sizeof(T); + if (m_currentOffset + bytes <= m_size) + { + char* dest = (char*)&val; + memcpy(dest, &m_data[m_currentOffset], bytes); + m_currentOffset += bytes; + return true; + } + btAssert(0); + return false; + } +}; + +bool btMiniSDF::load(const char* data, int size) +{ + int fileSize = -1; + + btSdfDataStream ds(data, size); + { + double buf[6]; + ds.read(buf); + m_domain.m_min[0] = buf[0]; + m_domain.m_min[1] = buf[1]; + m_domain.m_min[2] = buf[2]; + m_domain.m_min[3] = 0; + m_domain.m_max[0] = buf[3]; + m_domain.m_max[1] = buf[4]; + m_domain.m_max[2] = buf[5]; + m_domain.m_max[3] = 0; + } + { + unsigned int buf2[3]; + ds.read(buf2); + m_resolution[0] = buf2[0]; + m_resolution[1] = buf2[1]; + m_resolution[2] = buf2[2]; + } + { + double buf[3]; + ds.read(buf); + m_cell_size[0] = buf[0]; + m_cell_size[1] = buf[1]; + m_cell_size[2] = buf[2]; + } + { + double buf[3]; + ds.read(buf); + m_inv_cell_size[0] = buf[0]; + m_inv_cell_size[1] = buf[1]; + m_inv_cell_size[2] = buf[2]; + } + { + unsigned long long int cells; + ds.read(cells); + m_n_cells = cells; + } + { + unsigned long long int fields; + ds.read(fields); + m_n_fields = fields; + } + + unsigned long long int nodes0; + std::size_t n_nodes0; + ds.read(nodes0); + n_nodes0 = nodes0; + if (n_nodes0 > 1024 * 1024 * 1024) + { + return m_isValid; + } + m_nodes.resize(n_nodes0); + for (unsigned int i = 0; i < n_nodes0; i++) + { + unsigned long long int n_nodes1; + ds.read(n_nodes1); + btAlignedObjectArray<double>& nodes = m_nodes[i]; + nodes.resize(n_nodes1); + for (int j = 0; j < nodes.size(); j++) + { + double& node = nodes[j]; + ds.read(node); + } + } + + unsigned long long int n_cells0; + ds.read(n_cells0); + m_cells.resize(n_cells0); + for (int i = 0; i < n_cells0; i++) + { + unsigned long long int n_cells1; + btAlignedObjectArray<btCell32>& cells = m_cells[i]; + ds.read(n_cells1); + cells.resize(n_cells1); + for (int j = 0; j < n_cells1; j++) + { + btCell32& cell = cells[j]; + ds.read(cell); + } + } + + { + unsigned long long int n_cell_maps0; + ds.read(n_cell_maps0); + + m_cell_map.resize(n_cell_maps0); + for (int i = 0; i < n_cell_maps0; i++) + { + unsigned long long int n_cell_maps1; + btAlignedObjectArray<unsigned int>& cell_maps = m_cell_map[i]; + ds.read(n_cell_maps1); + cell_maps.resize(n_cell_maps1); + for (int j = 0; j < n_cell_maps1; j++) + { + unsigned int& cell_map = cell_maps[j]; + ds.read(cell_map); + } + } + } + + m_isValid = (ds.m_currentOffset == ds.m_size); + return m_isValid; +} + +unsigned int btMiniSDF::multiToSingleIndex(btMultiIndex const& ijk) const +{ + return m_resolution[1] * m_resolution[0] * ijk.ijk[2] + m_resolution[0] * ijk.ijk[1] + ijk.ijk[0]; +} + +btAlignedBox3d +btMiniSDF::subdomain(btMultiIndex const& ijk) const +{ + btAssert(m_isValid); + btVector3 tmp; + tmp.m_floats[0] = m_cell_size[0] * (double)ijk.ijk[0]; + tmp.m_floats[1] = m_cell_size[1] * (double)ijk.ijk[1]; + tmp.m_floats[2] = m_cell_size[2] * (double)ijk.ijk[2]; + + btVector3 origin = m_domain.min() + tmp; + + btAlignedBox3d box = btAlignedBox3d(origin, origin + m_cell_size); + return box; +} + +btMultiIndex +btMiniSDF::singleToMultiIndex(unsigned int l) const +{ + btAssert(m_isValid); + unsigned int n01 = m_resolution[0] * m_resolution[1]; + unsigned int k = l / n01; + unsigned int temp = l % n01; + unsigned int j = temp / m_resolution[0]; + unsigned int i = temp % m_resolution[0]; + btMultiIndex mi; + mi.ijk[0] = i; + mi.ijk[1] = j; + mi.ijk[2] = k; + return mi; +} + +btAlignedBox3d +btMiniSDF::subdomain(unsigned int l) const +{ + btAssert(m_isValid); + return subdomain(singleToMultiIndex(l)); +} + +btShapeMatrix +btMiniSDF::shape_function_(btVector3 const& xi, btShapeGradients* gradient) const +{ + btAssert(m_isValid); + btShapeMatrix res; + + btScalar x = xi[0]; + btScalar y = xi[1]; + btScalar z = xi[2]; + + btScalar x2 = x * x; + btScalar y2 = y * y; + btScalar z2 = z * z; + + btScalar _1mx = 1.0 - x; + btScalar _1my = 1.0 - y; + btScalar _1mz = 1.0 - z; + + btScalar _1px = 1.0 + x; + btScalar _1py = 1.0 + y; + btScalar _1pz = 1.0 + z; + + btScalar _1m3x = 1.0 - 3.0 * x; + btScalar _1m3y = 1.0 - 3.0 * y; + btScalar _1m3z = 1.0 - 3.0 * z; + + btScalar _1p3x = 1.0 + 3.0 * x; + btScalar _1p3y = 1.0 + 3.0 * y; + btScalar _1p3z = 1.0 + 3.0 * z; + + btScalar _1mxt1my = _1mx * _1my; + btScalar _1mxt1py = _1mx * _1py; + btScalar _1pxt1my = _1px * _1my; + btScalar _1pxt1py = _1px * _1py; + + btScalar _1mxt1mz = _1mx * _1mz; + btScalar _1mxt1pz = _1mx * _1pz; + btScalar _1pxt1mz = _1px * _1mz; + btScalar _1pxt1pz = _1px * _1pz; + + btScalar _1myt1mz = _1my * _1mz; + btScalar _1myt1pz = _1my * _1pz; + btScalar _1pyt1mz = _1py * _1mz; + btScalar _1pyt1pz = _1py * _1pz; + + btScalar _1mx2 = 1.0 - x2; + btScalar _1my2 = 1.0 - y2; + btScalar _1mz2 = 1.0 - z2; + + // Corner nodes. + btScalar fac = 1.0 / 64.0 * (9.0 * (x2 + y2 + z2) - 19.0); + res[0] = fac * _1mxt1my * _1mz; + res[1] = fac * _1pxt1my * _1mz; + res[2] = fac * _1mxt1py * _1mz; + res[3] = fac * _1pxt1py * _1mz; + res[4] = fac * _1mxt1my * _1pz; + res[5] = fac * _1pxt1my * _1pz; + res[6] = fac * _1mxt1py * _1pz; + res[7] = fac * _1pxt1py * _1pz; + + // Edge nodes. + + fac = 9.0 / 64.0 * _1mx2; + btScalar fact1m3x = fac * _1m3x; + btScalar fact1p3x = fac * _1p3x; + res[8] = fact1m3x * _1myt1mz; + res[9] = fact1p3x * _1myt1mz; + res[10] = fact1m3x * _1myt1pz; + res[11] = fact1p3x * _1myt1pz; + res[12] = fact1m3x * _1pyt1mz; + res[13] = fact1p3x * _1pyt1mz; + res[14] = fact1m3x * _1pyt1pz; + res[15] = fact1p3x * _1pyt1pz; + + fac = 9.0 / 64.0 * _1my2; + btScalar fact1m3y = fac * _1m3y; + btScalar fact1p3y = fac * _1p3y; + res[16] = fact1m3y * _1mxt1mz; + res[17] = fact1p3y * _1mxt1mz; + res[18] = fact1m3y * _1pxt1mz; + res[19] = fact1p3y * _1pxt1mz; + res[20] = fact1m3y * _1mxt1pz; + res[21] = fact1p3y * _1mxt1pz; + res[22] = fact1m3y * _1pxt1pz; + res[23] = fact1p3y * _1pxt1pz; + + fac = 9.0 / 64.0 * _1mz2; + btScalar fact1m3z = fac * _1m3z; + btScalar fact1p3z = fac * _1p3z; + res[24] = fact1m3z * _1mxt1my; + res[25] = fact1p3z * _1mxt1my; + res[26] = fact1m3z * _1mxt1py; + res[27] = fact1p3z * _1mxt1py; + res[28] = fact1m3z * _1pxt1my; + res[29] = fact1p3z * _1pxt1my; + res[30] = fact1m3z * _1pxt1py; + res[31] = fact1p3z * _1pxt1py; + + if (gradient) + { + btShapeGradients& dN = *gradient; + + btScalar _9t3x2py2pz2m19 = 9.0 * (3.0 * x2 + y2 + z2) - 19.0; + btScalar _9tx2p3y2pz2m19 = 9.0 * (x2 + 3.0 * y2 + z2) - 19.0; + btScalar _9tx2py2p3z2m19 = 9.0 * (x2 + y2 + 3.0 * z2) - 19.0; + btScalar _18x = 18.0 * x; + btScalar _18y = 18.0 * y; + btScalar _18z = 18.0 * z; + + btScalar _3m9x2 = 3.0 - 9.0 * x2; + btScalar _3m9y2 = 3.0 - 9.0 * y2; + btScalar _3m9z2 = 3.0 - 9.0 * z2; + + btScalar _2x = 2.0 * x; + btScalar _2y = 2.0 * y; + btScalar _2z = 2.0 * z; + + btScalar _18xm9t3x2py2pz2m19 = _18x - _9t3x2py2pz2m19; + btScalar _18xp9t3x2py2pz2m19 = _18x + _9t3x2py2pz2m19; + btScalar _18ym9tx2p3y2pz2m19 = _18y - _9tx2p3y2pz2m19; + btScalar _18yp9tx2p3y2pz2m19 = _18y + _9tx2p3y2pz2m19; + btScalar _18zm9tx2py2p3z2m19 = _18z - _9tx2py2p3z2m19; + btScalar _18zp9tx2py2p3z2m19 = _18z + _9tx2py2p3z2m19; + + dN(0, 0) = _18xm9t3x2py2pz2m19 * _1myt1mz; + dN(0, 1) = _1mxt1mz * _18ym9tx2p3y2pz2m19; + dN(0, 2) = _1mxt1my * _18zm9tx2py2p3z2m19; + dN(1, 0) = _18xp9t3x2py2pz2m19 * _1myt1mz; + dN(1, 1) = _1pxt1mz * _18ym9tx2p3y2pz2m19; + dN(1, 2) = _1pxt1my * _18zm9tx2py2p3z2m19; + dN(2, 0) = _18xm9t3x2py2pz2m19 * _1pyt1mz; + dN(2, 1) = _1mxt1mz * _18yp9tx2p3y2pz2m19; + dN(2, 2) = _1mxt1py * _18zm9tx2py2p3z2m19; + dN(3, 0) = _18xp9t3x2py2pz2m19 * _1pyt1mz; + dN(3, 1) = _1pxt1mz * _18yp9tx2p3y2pz2m19; + dN(3, 2) = _1pxt1py * _18zm9tx2py2p3z2m19; + dN(4, 0) = _18xm9t3x2py2pz2m19 * _1myt1pz; + dN(4, 1) = _1mxt1pz * _18ym9tx2p3y2pz2m19; + dN(4, 2) = _1mxt1my * _18zp9tx2py2p3z2m19; + dN(5, 0) = _18xp9t3x2py2pz2m19 * _1myt1pz; + dN(5, 1) = _1pxt1pz * _18ym9tx2p3y2pz2m19; + dN(5, 2) = _1pxt1my * _18zp9tx2py2p3z2m19; + dN(6, 0) = _18xm9t3x2py2pz2m19 * _1pyt1pz; + dN(6, 1) = _1mxt1pz * _18yp9tx2p3y2pz2m19; + dN(6, 2) = _1mxt1py * _18zp9tx2py2p3z2m19; + dN(7, 0) = _18xp9t3x2py2pz2m19 * _1pyt1pz; + dN(7, 1) = _1pxt1pz * _18yp9tx2p3y2pz2m19; + dN(7, 2) = _1pxt1py * _18zp9tx2py2p3z2m19; + + dN.topRowsDivide(8, 64.0); + + btScalar _m3m9x2m2x = -_3m9x2 - _2x; + btScalar _p3m9x2m2x = _3m9x2 - _2x; + btScalar _1mx2t1m3x = _1mx2 * _1m3x; + btScalar _1mx2t1p3x = _1mx2 * _1p3x; + dN(8, 0) = _m3m9x2m2x * _1myt1mz, + dN(8, 1) = -_1mx2t1m3x * _1mz, + dN(8, 2) = -_1mx2t1m3x * _1my; + dN(9, 0) = _p3m9x2m2x * _1myt1mz, + dN(9, 1) = -_1mx2t1p3x * _1mz, + dN(9, 2) = -_1mx2t1p3x * _1my; + dN(10, 0) = _m3m9x2m2x * _1myt1pz, + dN(10, 1) = -_1mx2t1m3x * _1pz, + dN(10, 2) = _1mx2t1m3x * _1my; + dN(11, 0) = _p3m9x2m2x * _1myt1pz, + dN(11, 1) = -_1mx2t1p3x * _1pz, + dN(11, 2) = _1mx2t1p3x * _1my; + dN(12, 0) = _m3m9x2m2x * _1pyt1mz, + dN(12, 1) = _1mx2t1m3x * _1mz, + dN(12, 2) = -_1mx2t1m3x * _1py; + dN(13, 0) = _p3m9x2m2x * _1pyt1mz, + dN(13, 1) = _1mx2t1p3x * _1mz, + dN(13, 2) = -_1mx2t1p3x * _1py; + dN(14, 0) = _m3m9x2m2x * _1pyt1pz, + dN(14, 1) = _1mx2t1m3x * _1pz, + dN(14, 2) = _1mx2t1m3x * _1py; + dN(15, 0) = _p3m9x2m2x * _1pyt1pz, + dN(15, 1) = _1mx2t1p3x * _1pz, + dN(15, 2) = _1mx2t1p3x * _1py; + + btScalar _m3m9y2m2y = -_3m9y2 - _2y; + btScalar _p3m9y2m2y = _3m9y2 - _2y; + btScalar _1my2t1m3y = _1my2 * _1m3y; + btScalar _1my2t1p3y = _1my2 * _1p3y; + dN(16, 0) = -_1my2t1m3y * _1mz, + dN(16, 1) = _m3m9y2m2y * _1mxt1mz, + dN(16, 2) = -_1my2t1m3y * _1mx; + dN(17, 0) = -_1my2t1p3y * _1mz, + dN(17, 1) = _p3m9y2m2y * _1mxt1mz, + dN(17, 2) = -_1my2t1p3y * _1mx; + dN(18, 0) = _1my2t1m3y * _1mz, + dN(18, 1) = _m3m9y2m2y * _1pxt1mz, + dN(18, 2) = -_1my2t1m3y * _1px; + dN(19, 0) = _1my2t1p3y * _1mz, + dN(19, 1) = _p3m9y2m2y * _1pxt1mz, + dN(19, 2) = -_1my2t1p3y * _1px; + dN(20, 0) = -_1my2t1m3y * _1pz, + dN(20, 1) = _m3m9y2m2y * _1mxt1pz, + dN(20, 2) = _1my2t1m3y * _1mx; + dN(21, 0) = -_1my2t1p3y * _1pz, + dN(21, 1) = _p3m9y2m2y * _1mxt1pz, + dN(21, 2) = _1my2t1p3y * _1mx; + dN(22, 0) = _1my2t1m3y * _1pz, + dN(22, 1) = _m3m9y2m2y * _1pxt1pz, + dN(22, 2) = _1my2t1m3y * _1px; + dN(23, 0) = _1my2t1p3y * _1pz, + dN(23, 1) = _p3m9y2m2y * _1pxt1pz, + dN(23, 2) = _1my2t1p3y * _1px; + + btScalar _m3m9z2m2z = -_3m9z2 - _2z; + btScalar _p3m9z2m2z = _3m9z2 - _2z; + btScalar _1mz2t1m3z = _1mz2 * _1m3z; + btScalar _1mz2t1p3z = _1mz2 * _1p3z; + dN(24, 0) = -_1mz2t1m3z * _1my, + dN(24, 1) = -_1mz2t1m3z * _1mx, + dN(24, 2) = _m3m9z2m2z * _1mxt1my; + dN(25, 0) = -_1mz2t1p3z * _1my, + dN(25, 1) = -_1mz2t1p3z * _1mx, + dN(25, 2) = _p3m9z2m2z * _1mxt1my; + dN(26, 0) = -_1mz2t1m3z * _1py, + dN(26, 1) = _1mz2t1m3z * _1mx, + dN(26, 2) = _m3m9z2m2z * _1mxt1py; + dN(27, 0) = -_1mz2t1p3z * _1py, + dN(27, 1) = _1mz2t1p3z * _1mx, + dN(27, 2) = _p3m9z2m2z * _1mxt1py; + dN(28, 0) = _1mz2t1m3z * _1my, + dN(28, 1) = -_1mz2t1m3z * _1px, + dN(28, 2) = _m3m9z2m2z * _1pxt1my; + dN(29, 0) = _1mz2t1p3z * _1my, + dN(29, 1) = -_1mz2t1p3z * _1px, + dN(29, 2) = _p3m9z2m2z * _1pxt1my; + dN(30, 0) = _1mz2t1m3z * _1py, + dN(30, 1) = _1mz2t1m3z * _1px, + dN(30, 2) = _m3m9z2m2z * _1pxt1py; + dN(31, 0) = _1mz2t1p3z * _1py, + dN(31, 1) = _1mz2t1p3z * _1px, + dN(31, 2) = _p3m9z2m2z * _1pxt1py; + + dN.bottomRowsMul(32u - 8u, 9.0 / 64.0); + } + + return res; +} + +bool btMiniSDF::interpolate(unsigned int field_id, double& dist, btVector3 const& x, + btVector3* gradient) const +{ + btAssert(m_isValid); + if (!m_isValid) + return false; + + if (!m_domain.contains(x)) + return false; + + btVector3 tmpmi = ((x - m_domain.min()) * (m_inv_cell_size)); //.cast<unsigned int>().eval(); + unsigned int mi[3] = {(unsigned int)tmpmi[0], (unsigned int)tmpmi[1], (unsigned int)tmpmi[2]}; + if (mi[0] >= m_resolution[0]) + mi[0] = m_resolution[0] - 1; + if (mi[1] >= m_resolution[1]) + mi[1] = m_resolution[1] - 1; + if (mi[2] >= m_resolution[2]) + mi[2] = m_resolution[2] - 1; + btMultiIndex mui; + mui.ijk[0] = mi[0]; + mui.ijk[1] = mi[1]; + mui.ijk[2] = mi[2]; + int i = multiToSingleIndex(mui); + unsigned int i_ = m_cell_map[field_id][i]; + if (i_ == UINT_MAX) + return false; + + btAlignedBox3d sd = subdomain(i); + i = i_; + btVector3 d = sd.m_max - sd.m_min; //.diagonal().eval(); + + btVector3 denom = (sd.max() - sd.min()); + btVector3 c0 = btVector3(2.0, 2.0, 2.0) / denom; + btVector3 c1 = (sd.max() + sd.min()) / denom; + btVector3 xi = (c0 * x - c1); + + btCell32 const& cell = m_cells[field_id][i]; + if (!gradient) + { + //auto phi = m_coefficients[field_id][i].dot(shape_function_(xi, 0)); + double phi = 0.0; + btShapeMatrix N = shape_function_(xi, 0); + for (unsigned int j = 0u; j < 32u; ++j) + { + unsigned int v = cell.m_cells[j]; + double c = m_nodes[field_id][v]; + if (c == DBL_MAX) + { + return false; + ; + } + phi += c * N[j]; + } + + dist = phi; + return true; + } + + btShapeGradients dN; + btShapeMatrix N = shape_function_(xi, &dN); + + double phi = 0.0; + gradient->setZero(); + for (unsigned int j = 0u; j < 32u; ++j) + { + unsigned int v = cell.m_cells[j]; + double c = m_nodes[field_id][v]; + if (c == DBL_MAX) + { + gradient->setZero(); + return false; + } + phi += c * N[j]; + (*gradient)[0] += c * dN(j, 0); + (*gradient)[1] += c * dN(j, 1); + (*gradient)[2] += c * dN(j, 2); + } + (*gradient) *= c0; + dist = phi; + return true; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.h new file mode 100644 index 00000000000..b60fd102fdc --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMiniSDF.h @@ -0,0 +1,127 @@ +#ifndef MINISDF_H +#define MINISDF_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAabbUtil2.h" +#include "LinearMath/btAlignedObjectArray.h" + +struct btMultiIndex +{ + unsigned int ijk[3]; +}; + +struct btAlignedBox3d +{ + btVector3 m_min; + btVector3 m_max; + + const btVector3& min() const + { + return m_min; + } + + const btVector3& max() const + { + return m_max; + } + + bool contains(const btVector3& x) const + { + return TestPointAgainstAabb2(m_min, m_max, x); + } + + btAlignedBox3d(const btVector3& mn, const btVector3& mx) + : m_min(mn), + m_max(mx) + { + } + + btAlignedBox3d() + { + } +}; + +struct btShapeMatrix +{ + double m_vec[32]; + + inline double& operator[](int i) + { + return m_vec[i]; + } + + inline const double& operator[](int i) const + { + return m_vec[i]; + } +}; + +struct btShapeGradients +{ + btVector3 m_vec[32]; + + void topRowsDivide(int row, double denom) + { + for (int i = 0; i < row; i++) + { + m_vec[i] /= denom; + } + } + + void bottomRowsMul(int row, double val) + { + for (int i = 32 - row; i < 32; i++) + { + m_vec[i] *= val; + } + } + + inline btScalar& operator()(int i, int j) + { + return m_vec[i][j]; + } +}; + +struct btCell32 +{ + unsigned int m_cells[32]; +}; + +struct btMiniSDF +{ + btAlignedBox3d m_domain; + unsigned int m_resolution[3]; + btVector3 m_cell_size; + btVector3 m_inv_cell_size; + std::size_t m_n_cells; + std::size_t m_n_fields; + bool m_isValid; + + btAlignedObjectArray<btAlignedObjectArray<double> > m_nodes; + btAlignedObjectArray<btAlignedObjectArray<btCell32> > m_cells; + btAlignedObjectArray<btAlignedObjectArray<unsigned int> > m_cell_map; + + btMiniSDF() + : m_isValid(false) + { + } + bool load(const char* data, int size); + bool isValid() const + { + return m_isValid; + } + unsigned int multiToSingleIndex(btMultiIndex const& ijk) const; + + btAlignedBox3d subdomain(btMultiIndex const& ijk) const; + + btMultiIndex singleToMultiIndex(unsigned int l) const; + + btAlignedBox3d subdomain(unsigned int l) const; + + btShapeMatrix + shape_function_(btVector3 const& xi, btShapeGradients* gradient = 0) const; + + bool interpolate(unsigned int field_id, double& dist, btVector3 const& x, btVector3* gradient) const; +}; + +#endif //MINISDF_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 06707e24e55..d4b6a651de0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -13,48 +13,59 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btMinkowskiSumShape.h" - -btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB) -: btConvexInternalShape (), -m_shapeA(shapeA), -m_shapeB(shapeB) +btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA, const btConvexShape* shapeB) + : btConvexInternalShape(), + m_shapeA(shapeA), + m_shapeB(shapeB) { m_shapeType = MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE; m_transA.setIdentity(); m_transB.setIdentity(); } -btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis())); - btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec*m_transB.getBasis())); - return supVertexA - supVertexB; + btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec * m_transA.getBasis())); + btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec * m_transB.getBasis())); + return supVertexA - supVertexB; } -void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { ///@todo: could make recursive use of batching. probably this shape is not used frequently. - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { supportVerticesOut[i] = localGetSupportingVertexWithoutMargin(vectors[i]); } - } - - -btScalar btMinkowskiSumShape::getMargin() const +btScalar btMinkowskiSumShape::getMargin() const { return m_shapeA->getMargin() + m_shapeB->getMargin(); } - -void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btMinkowskiSumShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { (void)mass; - btAssert(0); - inertia.setValue(0,0,0); + //inertia of the AABB of the Minkowski sum + btTransform identity; + identity.setIdentity(); + btVector3 aabbMin, aabbMax; + getAabb(identity, aabbMin, aabbMax); + + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); + + btScalar margin = getMargin(); + + btScalar lx = btScalar(2.) * (halfExtents.x() + margin); + btScalar ly = btScalar(2.) * (halfExtents.y() + margin); + btScalar lz = btScalar(2.) * (halfExtents.z() + margin); + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; + const btScalar scaledmass = mass * btScalar(0.08333333); + + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index a3f9a472394..3b5150f6d50 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -17,46 +17,43 @@ subject to the following restrictions: #define BT_MINKOWSKI_SUM_SHAPE_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types /// The btMinkowskiSumShape is only for advanced users. This shape represents implicit based minkowski sum of two convex implicit shapes. -ATTRIBUTE_ALIGNED16(class) btMinkowskiSumShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btMinkowskiSumShape : public btConvexInternalShape { - - btTransform m_transA; - btTransform m_transB; - const btConvexShape* m_shapeA; - const btConvexShape* m_shapeB; + btTransform m_transA; + btTransform m_transB; + const btConvexShape* m_shapeA; + const btConvexShape* m_shapeB; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); -BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + btMinkowskiSumShape(const btConvexShape* shapeA, const btConvexShape* shapeB); + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - void setTransformA(const btTransform& transA) { m_transA = transA;} - void setTransformB(const btTransform& transB) { m_transB = transB;} + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - const btTransform& getTransformA()const { return m_transA;} - const btTransform& GetTransformB()const { return m_transB;} + void setTransformA(const btTransform& transA) { m_transA = transA; } + void setTransformB(const btTransform& transB) { m_transB = transB; } + const btTransform& getTransformA() const { return m_transA; } + const btTransform& GetTransformB() const { return m_transB; } - virtual btScalar getMargin() const; + virtual btScalar getMargin() const; - const btConvexShape* getShapeA() const { return m_shapeA;} - const btConvexShape* getShapeB() const { return m_shapeB;} + const btConvexShape* getShapeA() const { return m_shapeA; } + const btConvexShape* getShapeB() const { return m_shapeB; } - virtual const char* getName()const + virtual const char* getName() const { return "MinkowskiSum"; } }; -#endif //BT_MINKOWSKI_SUM_SHAPE_H +#endif //BT_MINKOWSKI_SUM_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index 6abfdffa677..c0cc55dfb08 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -13,52 +13,50 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//#if defined (_WIN32) || defined (__i386__) -//#define BT_USE_SSE_IN_API -//#endif +#if defined(_WIN32) || defined(__i386__) +#define BT_USE_SSE_IN_API +#endif #include "btMultiSphereShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" #include "LinearMath/btSerializer.h" -btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres) -:btConvexInternalAabbCachingShape () +btMultiSphereShape::btMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres) + : btConvexInternalAabbCachingShape() { m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE; //btScalar startMargin = btScalar(BT_LARGE_FLOAT); m_localPositionArray.resize(numSpheres); m_radiArray.resize(numSpheres); - for (int i=0;i<numSpheres;i++) + for (int i = 0; i < numSpheres; i++) { m_localPositionArray[i] = positions[i]; m_radiArray[i] = radi[i]; - } recalcLocalAabb(); - } #ifndef MIN - #define MIN( _a, _b) ((_a) < (_b) ? (_a) : (_b)) +#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b)) #endif - btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - btVector3 supVec(0,0,0); + btVector3 supVec(0, 0, 0); btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); - btVector3 vec = vec0; btScalar lenSqr = vec.length2(); - if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON)) + if (lenSqr < (SIMD_EPSILON * SIMD_EPSILON)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } @@ -69,32 +67,30 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala const btScalar* rad = &m_radiArray[0]; int numSpheres = m_localPositionArray.size(); - for( int k = 0; k < numSpheres; k+= 128 ) + for (int k = 0; k < numSpheres; k += 128) { btVector3 temp[128]; - int inner_count = MIN( numSpheres - k, 128 ); - for( long i = 0; i < inner_count; i++ ) - { - temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin(); - pos++; - rad++; - } - long i = vec.maxDot( temp, inner_count, newDot); - if( newDot > maxDot ) + int inner_count = MIN(numSpheres - k, 128); + for (long i = 0; i < inner_count; i++) + { + temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin(); + pos++; + rad++; + } + long i = vec.maxDot(temp, inner_count, newDot); + if (newDot > maxDot) { maxDot = newDot; supVec = temp[i]; } - } + } return supVec; - } - void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - - for (int j=0;j<numVectors;j++) + for (int j = 0; j < numVectors; j++) { btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); @@ -107,76 +103,67 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala const btScalar* rad = &m_radiArray[0]; int numSpheres = m_localPositionArray.size(); - for( int k = 0; k < numSpheres; k+= 128 ) - { - btVector3 temp[128]; - int inner_count = MIN( numSpheres - k, 128 ); - for( long i = 0; i < inner_count; i++ ) - { - temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin(); - pos++; - rad++; - } - long i = vec.maxDot( temp, inner_count, newDot); - if( newDot > maxDot ) - { - maxDot = newDot; - supportVerticesOut[j] = temp[i]; - } - } - + for (int k = 0; k < numSpheres; k += 128) + { + btVector3 temp[128]; + int inner_count = MIN(numSpheres - k, 128); + for (long i = 0; i < inner_count; i++) + { + temp[i] = (*pos) * m_localScaling + vec * m_localScaling * (*rad) - vec * getMargin(); + pos++; + rad++; + } + long i = vec.maxDot(temp, inner_count, newDot); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = temp[i]; + } + } } } - - - - - - - -void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btMultiSphereShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { //as an approximation, take the inertia of the box that bounds the spheres - btVector3 localAabbMin,localAabbMax; - getCachedLocalAabb(localAabbMin,localAabbMax); - btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5); - - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); + btVector3 localAabbMin, localAabbMax; + getCachedLocalAabb(localAabbMin, localAabbMax); + btVector3 halfExtents = (localAabbMax - localAabbMin) * btScalar(0.5); - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer; + btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*)dataBuffer; btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer); int numElem = m_localPositionArray.size(); - shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]): 0; - + shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]) : 0; + shapeData->m_localPositionArraySize = numElem; if (numElem) { - btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem); + btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius), numElem); btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { m_localPositionArray[i].serializeFloat(memPtr->m_pos); memPtr->m_radius = float(m_radiArray[i]); } - serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]); + serializer->finalizeChunk(chunk, "btPositionAndRadius", BT_ARRAY_CODE, (void*)&m_localPositionArray[0]); } - - return "btMultiSphereShapeData"; -} + // Fill padding with zeros to appease msan. + memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding)); + return "btMultiSphereShapeData"; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index 5d3b4026848..2d79c07ca42 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -17,69 +17,65 @@ subject to the following restrictions: #define BT_MULTI_SPHERE_MINKOWSKI_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAabbUtil2.h" - - ///The btMultiSphereShape represents the convex hull of a collection of spheres. You can create special capsules or other smooth volumes. ///It is possible to animate the spheres for deformation, but call 'recalcLocalAabb' after changing any sphere position/radius -ATTRIBUTE_ALIGNED16(class) btMultiSphereShape : public btConvexInternalAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btMultiSphereShape : public btConvexInternalAabbCachingShape { - btAlignedObjectArray<btVector3> m_localPositionArray; - btAlignedObjectArray<btScalar> m_radiArray; - + btAlignedObjectArray<btScalar> m_radiArray; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres); + + btMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres); ///CollisionShape Interface - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; /// btConvexShape Interface - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - int getSphereCount() const + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + + int getSphereCount() const { return m_localPositionArray.size(); } - const btVector3& getSpherePosition(int index) const + const btVector3& getSpherePosition(int index) const { return m_localPositionArray[index]; } - btScalar getSphereRadius(int index) const + btScalar getSphereRadius(int index) const { return m_radiArray[index]; } - - virtual const char* getName()const + virtual const char* getName() const { return "MultiSphere"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; - -struct btPositionAndRadius +struct btPositionAndRadius { - btVector3FloatData m_pos; - float m_radius; + btVector3FloatData m_pos; + float m_radius; }; +// clang-format off + struct btMultiSphereShapeData { btConvexInternalShapeData m_convexInternalShapeData; @@ -89,13 +85,11 @@ struct btMultiSphereShapeData char m_padding[4]; }; +// clang-format on - -SIMD_FORCE_INLINE int btMultiSphereShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btMultiSphereShape::calculateSerializeBufferSize() const { return sizeof(btMultiSphereShapeData); } - - -#endif //BT_MULTI_SPHERE_MINKOWSKI_H +#endif //BT_MULTI_SPHERE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp index 58799ac96ad..30108c9e7b4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp @@ -19,27 +19,25 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h" //#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" - ///Obtains the material for a specific triangle -const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) +const btMaterial *btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) { - const unsigned char * materialBase = 0; - int numMaterials; - PHY_ScalarType materialType; - int materialStride; - const unsigned char * triangleMaterialBase = 0; - int numTriangles; - int triangleMaterialStride; - PHY_ScalarType triangleType; - - ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, - &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); - - // return the pointer to the place with the friction for the triangle - // TODO: This depends on whether it's a moving mesh or not - // BUG IN GIMPACT - //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); - int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); - btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); - return (matVal); + const unsigned char *materialBase = 0; + int numMaterials; + PHY_ScalarType materialType; + int materialStride; + const unsigned char *triangleMaterialBase = 0; + int numTriangles; + int triangleMaterialStride; + PHY_ScalarType triangleType; + + ((btTriangleIndexVertexMaterialArray *)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); + + // return the pointer to the place with the friction for the triangle + // TODO: This depends on whether it's a moving mesh or not + // BUG IN GIMPACT + //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); + int *matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); + btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); + return (matVal); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h index 5ebaede4a88..d1d42f8e049 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -22,82 +22,80 @@ subject to the following restrictions: #include "btMaterial.h" ///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution. -ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape +ATTRIBUTE_ALIGNED16(class) +btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape { - btAlignedObjectArray <btMaterial*> m_materialList; + btAlignedObjectArray<btMaterial *> m_materialList; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true): - btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) - { - m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - - const unsigned char *vertexbase; - int numverts; - PHY_ScalarType type; - int stride; - const unsigned char *indexbase; - int indexstride; - int numfaces; - PHY_ScalarType indicestype; - - //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); - - for(int i = 0; i < meshInterface->getNumSubParts(); i++) - { - m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase, - numverts, - type, - stride, - &indexbase, - indexstride, - numfaces, - indicestype, - i); - //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16)); - } - } + btMultimaterialTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true) : btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) + { + m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); + + for (int i = 0; i < meshInterface->getNumSubParts(); i++) + { + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + i); + //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16)); + } + } ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb - btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true): - btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh) - { - m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - - const unsigned char *vertexbase; - int numverts; - PHY_ScalarType type; - int stride; - const unsigned char *indexbase; - int indexstride; - int numfaces; - PHY_ScalarType indicestype; - - //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); - - for(int i = 0; i < meshInterface->getNumSubParts(); i++) - { - m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase, - numverts, - type, - stride, - &indexbase, - indexstride, - numfaces, - indicestype, - i); - //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16)); - } - } - - virtual ~btMultimaterialTriangleMeshShape() - { -/* + btMultimaterialTriangleMeshShape(btStridingMeshInterface * meshInterface, bool useQuantizedAabbCompression, const btVector3 &bvhAabbMin, const btVector3 &bvhAabbMax, bool buildBvh = true) : btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh) + { + m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); + + for (int i = 0; i < meshInterface->getNumSubParts(); i++) + { + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + i); + //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16)); + } + } + + virtual ~btMultimaterialTriangleMeshShape() + { + /* for(int i = 0; i < m_meshInterface->getNumSubParts(); i++) { btAlignedFree(m_materialValues[i]); @@ -106,14 +104,12 @@ public: btAlignedFree(m_materialValues); m_materialLookup = NULL; */ - } + } //debugging - virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";} - - ///Obtains the material for a specific triangle - const btMaterial * getMaterialProperties(int partID, int triIndex); + virtual const char *getName() const { return "MULTIMATERIALTRIANGLEMESH"; } -} -; + ///Obtains the material for a specific triangle + const btMaterial *getMaterialProperties(int partID, int triIndex); +}; -#endif //BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H +#endif //BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 6f36775f7c9..863ea6d6ac2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -13,51 +13,46 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btOptimizedBvh.h" #include "btStridingMeshInterface.h" #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btIDebugDraw.h" - btOptimizedBvh::btOptimizedBvh() -{ +{ } btOptimizedBvh::~btOptimizedBvh() { } - void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax) { m_useQuantization = useQuantizedAabbCompression; - // NodeArray triangleNodes; - struct NodeTriangleCallback : public btInternalTriangleIndexCallback + struct NodeTriangleCallback : public btInternalTriangleIndexCallback { - - NodeArray& m_triangleNodes; + NodeArray& m_triangleNodes; NodeTriangleCallback& operator=(NodeTriangleCallback& other) { m_triangleNodes.copyFromArray(other.m_triangleNodes); return *this; } - - NodeTriangleCallback(NodeArray& triangleNodes) - :m_triangleNodes(triangleNodes) + + NodeTriangleCallback(NodeArray& triangleNodes) + : m_triangleNodes(triangleNodes) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { btOptimizedBvhNode node; - btVector3 aabbMin,aabbMax; - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + btVector3 aabbMin, aabbMax; + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangle[0]); aabbMax.setMax(triangle[0]); aabbMin.setMin(triangle[1]); @@ -70,17 +65,17 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized node.m_aabbMaxOrg = aabbMax; node.m_escapeIndex = -1; - + //for child nodes node.m_subPart = partId; node.m_triangleIndex = triangleIndex; m_triangleNodes.push_back(node); } }; - struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback + struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback { - QuantizedNodeArray& m_triangleNodes; - const btQuantizedBvh* m_optimizedTree; // for quantization + QuantizedNodeArray& m_triangleNodes; + const btQuantizedBvh* m_optimizedTree; // for quantization QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other) { @@ -89,23 +84,23 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized return *this; } - QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btQuantizedBvh* tree) - :m_triangleNodes(triangleNodes),m_optimizedTree(tree) + QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes, const btQuantizedBvh* tree) + : m_triangleNodes(triangleNodes), m_optimizedTree(tree) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { // The partId and triangle index must fit in the same (positive) integer - btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS)); - btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS))); + btAssert(partId < (1 << MAX_NUM_PARTS_IN_BITS)); + btAssert(triangleIndex < (1 << (31 - MAX_NUM_PARTS_IN_BITS))); //negative indices are reserved for escapeIndex - btAssert(triangleIndex>=0); + btAssert(triangleIndex >= 0); btQuantizedBvhNode node; - btVector3 aabbMin,aabbMax; - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + btVector3 aabbMin, aabbMax; + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangle[0]); aabbMax.setMax(triangle[0]); aabbMin.setMin(triangle[1]); @@ -132,59 +127,52 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION); } - m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); - m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); + m_optimizedTree->quantize(&node.m_quantizedAabbMin[0], aabbMin, 0); + m_optimizedTree->quantize(&node.m_quantizedAabbMax[0], aabbMax, 1); - node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; + node.m_escapeIndexOrTriangleIndex = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex; m_triangleNodes.push_back(node); } }; - - int numLeafNodes = 0; - if (m_useQuantization) { - //initialize quantization values - setQuantizationValues(bvhAabbMin,bvhAabbMax); + setQuantizationValues(bvhAabbMin, bvhAabbMax); - QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this); + QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes, this); - - triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax); + triangles->InternalProcessAllTriangles(&callback, m_bvhAabbMin, m_bvhAabbMax); //now we have an array of leafnodes in m_leafNodes numLeafNodes = m_quantizedLeafNodes.size(); - - m_quantizedContiguousNodes.resize(2*numLeafNodes); - - - } else + m_quantizedContiguousNodes.resize(2 * numLeafNodes); + } + else { - NodeTriangleCallback callback(m_leafNodes); + NodeTriangleCallback callback(m_leafNodes); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); - triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); + triangles->InternalProcessAllTriangles(&callback, aabbMin, aabbMax); //now we have an array of leafnodes in m_leafNodes numLeafNodes = m_leafNodes.size(); - m_contiguousNodes.resize(2*numLeafNodes); + m_contiguousNodes.resize(2 * numLeafNodes); } m_curNodeIndex = 0; - buildTree(0,numLeafNodes); + buildTree(0, numLeafNodes); ///if the entire tree is small then subtree size, we need to create a header info for the tree - if(m_useQuantization && !m_SubtreeHeaders.size()) + if (m_useQuantization && !m_SubtreeHeaders.size()) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); @@ -200,37 +188,29 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized m_leafNodes.clear(); } - - - -void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) +void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface, const btVector3& aabbMin, const btVector3& aabbMax) { if (m_useQuantization) { + setQuantizationValues(aabbMin, aabbMax); - setQuantizationValues(aabbMin,aabbMax); - - updateBvhNodes(meshInterface,0,m_curNodeIndex,0); + updateBvhNodes(meshInterface, 0, m_curNodeIndex, 0); ///now update all subtree headers int i; - for (i=0;i<m_SubtreeHeaders.size();i++) + for (i = 0; i < m_SubtreeHeaders.size(); i++) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); } - - } else + } + else { - } } - - - -void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) +void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface, const btVector3& aabbMin, const btVector3& aabbMax) { //incrementally initialize quantization values btAssert(m_useQuantization); @@ -245,147 +225,140 @@ void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const b ///we should update all quantization values, using updateBvhNodes(meshInterface); ///but we only update chunks that overlap the given aabb - - unsigned short quantizedQueryAabbMin[3]; - unsigned short quantizedQueryAabbMax[3]; - quantize(&quantizedQueryAabbMin[0],aabbMin,0); - quantize(&quantizedQueryAabbMax[0],aabbMax,1); + unsigned short quantizedQueryAabbMin[3]; + unsigned short quantizedQueryAabbMax[3]; + + quantize(&quantizedQueryAabbMin[0], aabbMin, 0); + quantize(&quantizedQueryAabbMax[0], aabbMax, 1); int i; - for (i=0;i<this->m_SubtreeHeaders.size();i++) + for (i = 0; i < this->m_SubtreeHeaders.size(); i++) { btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; //PCK: unsigned instead of bool - unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax); if (overlap != 0) { - updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i); + updateBvhNodes(meshInterface, subtree.m_rootNodeIndex, subtree.m_rootNodeIndex + subtree.m_subtreeSize, i); subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); } } - } -void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index) +void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int firstNode, int endNode, int index) { (void)index; btAssert(m_useQuantization); - int curNodeSubPart=-1; + int curNodeSubPart = -1; //get access info to trianglemesh data - const unsigned char *vertexbase = 0; - int numverts = 0; - PHY_ScalarType type = PHY_INTEGER; - int stride = 0; - const unsigned char *indexbase = 0; - int indexstride = 0; - int numfaces = 0; - PHY_ScalarType indicestype = PHY_INTEGER; - - btVector3 triangleVerts[3]; - btVector3 aabbMin,aabbMax; - const btVector3& meshScaling = meshInterface->getScaling(); - - int i; - for (i=endNode-1;i>=firstNode;i--) + const unsigned char* vertexbase = 0; + int numverts = 0; + PHY_ScalarType type = PHY_INTEGER; + int stride = 0; + const unsigned char* indexbase = 0; + int indexstride = 0; + int numfaces = 0; + PHY_ScalarType indicestype = PHY_INTEGER; + + btVector3 triangleVerts[3]; + btVector3 aabbMin, aabbMax; + const btVector3& meshScaling = meshInterface->getScaling(); + + int i; + for (i = endNode - 1; i >= firstNode; i--) + { + btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; + if (curNode.isLeafNode()) { + //recalc aabb from triangle data + int nodeSubPart = curNode.getPartId(); + int nodeTriangleIndex = curNode.getTriangleIndex(); + if (nodeSubPart != curNodeSubPart) + { + if (curNodeSubPart >= 0) + meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart); + curNodeSubPart = nodeSubPart; + } + //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, - btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; - if (curNode.isLeafNode()) + unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); + + for (int j = 2; j >= 0; j--) { - //recalc aabb from triangle data - int nodeSubPart = curNode.getPartId(); - int nodeTriangleIndex = curNode.getTriangleIndex(); - if (nodeSubPart != curNodeSubPart) + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } + if (type == PHY_FLOAT) { - if (curNodeSubPart >= 0) - meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); - meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart); - - curNodeSubPart = nodeSubPart; - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); + triangleVerts[j] = btVector3( + graphicsbase[0] * meshScaling.getX(), + graphicsbase[1] * meshScaling.getY(), + graphicsbase[2] * meshScaling.getZ()); } - //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, - - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - - - for (int j=2;j>=0;j--) + else { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - if (type == PHY_FLOAT) - { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); - } - else - { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); - } + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ())); } + } + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMin.setMin(triangleVerts[0]); + aabbMax.setMax(triangleVerts[0]); + aabbMin.setMin(triangleVerts[1]); + aabbMax.setMax(triangleVerts[1]); + aabbMin.setMin(triangleVerts[2]); + aabbMax.setMax(triangleVerts[2]); + + quantize(&curNode.m_quantizedAabbMin[0], aabbMin, 0); + quantize(&curNode.m_quantizedAabbMax[0], aabbMax, 1); + } + else + { + //combine aabb from both children - - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMin.setMin(triangleVerts[0]); - aabbMax.setMax(triangleVerts[0]); - aabbMin.setMin(triangleVerts[1]); - aabbMax.setMax(triangleVerts[1]); - aabbMin.setMin(triangleVerts[2]); - aabbMax.setMax(triangleVerts[2]); - - quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0); - quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1); - - } else - { - //combine aabb from both children + btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i + 1]; - btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1]; - - btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] : - &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()]; - + btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i + 2] : &m_quantizedContiguousNodes[i + 1 + leftChildNode->getEscapeIndex()]; + { + for (int i = 0; i < 3; i++) { - for (int i=0;i<3;i++) - { - curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i]; - if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i]) - curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i]; - - curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i]; - if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i]) - curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i]; - } + curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i]; + if (curNode.m_quantizedAabbMin[i] > rightChildNode->m_quantizedAabbMin[i]) + curNode.m_quantizedAabbMin[i] = rightChildNode->m_quantizedAabbMin[i]; + + curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i]; + if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i]) + curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i]; } } - } + } - if (curNodeSubPart >= 0) - meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); - - + if (curNodeSubPart >= 0) + meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); } ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' -btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) +btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) { - btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian); - + btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer, i_dataBufferSize, i_swapEndian); + //we don't add additional data so just do a static upcast return static_cast<btOptimizedBvh*>(bvh); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index 715961f5528..22f131c8b25 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -22,44 +22,35 @@ subject to the following restrictions: class btStridingMeshInterface; - ///The btOptimizedBvh extends the btQuantizedBvh to create AABB tree for triangle meshes, through the btStridingMeshInterface. -ATTRIBUTE_ALIGNED16(class) btOptimizedBvh : public btQuantizedBvh +ATTRIBUTE_ALIGNED16(class) +btOptimizedBvh : public btQuantizedBvh { - public: BT_DECLARE_ALIGNED_ALLOCATOR(); protected: - public: - btOptimizedBvh(); virtual ~btOptimizedBvh(); - void build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax); + void build(btStridingMeshInterface * triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax); - void refit(btStridingMeshInterface* triangles,const btVector3& aabbMin,const btVector3& aabbMax); + void refit(btStridingMeshInterface * triangles, const btVector3& aabbMin, const btVector3& aabbMax); - void refitPartial(btStridingMeshInterface* triangles,const btVector3& aabbMin, const btVector3& aabbMax); + void refitPartial(btStridingMeshInterface * triangles, const btVector3& aabbMin, const btVector3& aabbMax); - void updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index); + void updateBvhNodes(btStridingMeshInterface * meshInterface, int firstNode, int endNode, int index); /// Data buffer MUST be 16 byte aligned - virtual bool serializeInPlace(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const + virtual bool serializeInPlace(void* o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const { - return btQuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian); - + return btQuantizedBvh::serialize(o_alignedDataBuffer, i_dataBufferSize, i_swapEndian); } ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' - static btOptimizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); - - + static btOptimizedBvh* deSerializeInPlace(void* i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); }; - -#endif //BT_OPTIMIZED_BVH_H - - +#endif //BT_OPTIMIZED_BVH_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index b831e20c2f9..e4bd7bb4d56 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -12,9 +12,9 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ -//#if defined (_WIN32) || defined (__i386__) -//#define BT_USE_SSE_IN_API -//#endif +#if defined(_WIN32) || defined(__i386__) +#define BT_USE_SSE_IN_API +#endif #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" #include "btConvexPolyhedron.h" @@ -23,11 +23,9 @@ subject to the following restrictions: #include "LinearMath/btGeometryUtil.h" #include "LinearMath/btGrahamScan2dConvexHull.h" - -btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(), -m_polyhedron(0) +btPolyhedralConvexShape::btPolyhedralConvexShape() : btConvexInternalShape(), + m_polyhedron(0) { - } btPolyhedralConvexShape::~btPolyhedralConvexShape() @@ -39,79 +37,148 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape() } } - -bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin) +void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron) { + if (m_polyhedron) + { + *m_polyhedron = polyhedron; + } + else + { + void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16); + m_polyhedron = new (mem) btConvexPolyhedron(polyhedron); + } +} +bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin) +{ if (m_polyhedron) { m_polyhedron->~btConvexPolyhedron(); btAlignedFree(m_polyhedron); } - - void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16); + + void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron), 16); m_polyhedron = new (mem) btConvexPolyhedron; btAlignedObjectArray<btVector3> orgVertices; - for (int i=0;i<getNumVertices();i++) + for (int i = 0; i < getNumVertices(); i++) { btVector3& newVertex = orgVertices.expand(); - getVertex(i,newVertex); + getVertex(i, newVertex); } - + btConvexHullComputer conv; - + if (shiftVerticesByMargin) { btAlignedObjectArray<btVector3> planeEquations; - btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); + btGeometryUtil::getPlaneEquationsFromVertices(orgVertices, planeEquations); btAlignedObjectArray<btVector3> shiftedPlaneEquations; - for (int p=0;p<planeEquations.size();p++) + for (int p = 0; p < planeEquations.size(); p++) { - btVector3 plane = planeEquations[p]; - // btScalar margin = getMargin(); - plane[3] -= getMargin(); - shiftedPlaneEquations.push_back(plane); + btVector3 plane = planeEquations[p]; + // btScalar margin = getMargin(); + plane[3] -= getMargin(); + shiftedPlaneEquations.push_back(plane); } btAlignedObjectArray<btVector3> tmpVertices; - btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); - - conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); - } else + btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations, tmpVertices); + + conv.compute(&tmpVertices[0].getX(), sizeof(btVector3), tmpVertices.size(), 0.f, 0.f); + } + else { - - conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); + conv.compute(&orgVertices[0].getX(), sizeof(btVector3), orgVertices.size(), 0.f, 0.f); } +#ifndef BT_RECONSTRUCT_FACES + int numVertices = conv.vertices.size(); + m_polyhedron->m_vertices.resize(numVertices); + for (int p = 0; p < numVertices; p++) + { + m_polyhedron->m_vertices[p] = conv.vertices[p]; + } + + int v0, v1; + for (int j = 0; j < conv.faces.size(); j++) + { + btVector3 edges[3]; + int numEdges = 0; + btFace combinedFace; + const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]]; + v0 = edge->getSourceVertex(); + int prevVertex = v0; + combinedFace.m_indices.push_back(v0); + v1 = edge->getTargetVertex(); + while (v1 != v0) + { + btVector3 wa = conv.vertices[prevVertex]; + btVector3 wb = conv.vertices[v1]; + btVector3 newEdge = wb - wa; + newEdge.normalize(); + if (numEdges < 2) + edges[numEdges++] = newEdge; + + //face->addIndex(v1); + combinedFace.m_indices.push_back(v1); + edge = edge->getNextEdgeOfFace(); + prevVertex = v1; + int v01 = edge->getSourceVertex(); + v1 = edge->getTargetVertex(); + } + + btAssert(combinedFace.m_indices.size() > 2); + + btVector3 faceNormal = edges[0].cross(edges[1]); + faceNormal.normalize(); + + btScalar planeEq = 1e30f; + + for (int v = 0; v < combinedFace.m_indices.size(); v++) + { + btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal); + if (planeEq > eq) + { + planeEq = eq; + } + } + combinedFace.m_plane[0] = faceNormal.getX(); + combinedFace.m_plane[1] = faceNormal.getY(); + combinedFace.m_plane[2] = faceNormal.getZ(); + combinedFace.m_plane[3] = -planeEq; + + m_polyhedron->m_faces.push_back(combinedFace); + } + +#else //BT_RECONSTRUCT_FACES btAlignedObjectArray<btVector3> faceNormals; int numFaces = conv.faces.size(); faceNormals.resize(numFaces); btConvexHullComputer* convexUtil = &conv; - - btAlignedObjectArray<btFace> tmpFaces; + btAlignedObjectArray<btFace> tmpFaces; tmpFaces.resize(numFaces); int numVertices = convexUtil->vertices.size(); m_polyhedron->m_vertices.resize(numVertices); - for (int p=0;p<numVertices;p++) + for (int p = 0; p < numVertices; p++) { m_polyhedron->m_vertices[p] = convexUtil->vertices[p]; } - - for (int i=0;i<numFaces;i++) + for (int i = 0; i < numFaces; i++) { int face = convexUtil->faces[i]; //printf("face=%d\n",face); - const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; - const btConvexHullComputer::Edge* edge = firstEdge; + const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; + const btConvexHullComputer::Edge* edge = firstEdge; btVector3 edges[3]; int numEdges = 0; @@ -119,25 +186,23 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa do { - int src = edge->getSourceVertex(); tmpFaces[i].m_indices.push_back(src); int targ = edge->getTargetVertex(); btVector3 wa = convexUtil->vertices[src]; btVector3 wb = convexUtil->vertices[targ]; - btVector3 newEdge = wb-wa; + btVector3 newEdge = wb - wa; newEdge.normalize(); - if (numEdges<2) + if (numEdges < 2) edges[numEdges++] = newEdge; edge = edge->getNextEdgeOfFace(); - } while (edge!=firstEdge); + } while (edge != firstEdge); btScalar planeEq = 1e30f; - - if (numEdges==2) + if (numEdges == 2) { faceNormals[i] = edges[0].cross(edges[1]); faceNormals[i].normalize(); @@ -145,20 +210,19 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa tmpFaces[i].m_plane[1] = faceNormals[i].getY(); tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); tmpFaces[i].m_plane[3] = planeEq; - } else { - btAssert(0);//degenerate? + btAssert(0); //degenerate? faceNormals[i].setZero(); } - for (int v=0;v<tmpFaces[i].m_indices.size();v++) + for (int v = 0; v < tmpFaces[i].m_indices.size(); v++) { btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]); - if (planeEq>eq) + if (planeEq > eq) { - planeEq=eq; + planeEq = eq; } } tmpFaces[i].m_plane[3] = -planeEq; @@ -166,89 +230,86 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa //merge coplanar faces and copy them to m_polyhedron - btScalar faceWeldThreshold= 0.999f; + btScalar faceWeldThreshold = 0.999f; btAlignedObjectArray<int> todoFaces; - for (int i=0;i<tmpFaces.size();i++) + for (int i = 0; i < tmpFaces.size(); i++) todoFaces.push_back(i); while (todoFaces.size()) { btAlignedObjectArray<int> coplanarFaceGroup; - int refFace = todoFaces[todoFaces.size()-1]; + int refFace = todoFaces[todoFaces.size() - 1]; coplanarFaceGroup.push_back(refFace); btFace& faceA = tmpFaces[refFace]; todoFaces.pop_back(); - btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); - for (int j=todoFaces.size()-1;j>=0;j--) + btVector3 faceNormalA(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]); + for (int j = todoFaces.size() - 1; j >= 0; j--) { int i = todoFaces[j]; btFace& faceB = tmpFaces[i]; - btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); - if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) + btVector3 faceNormalB(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]); + if (faceNormalA.dot(faceNormalB) > faceWeldThreshold) { coplanarFaceGroup.push_back(i); todoFaces.remove(i); } } - bool did_merge = false; - if (coplanarFaceGroup.size()>1) + if (coplanarFaceGroup.size() > 1) { //do the merge: use Graham Scan 2d convex hull btAlignedObjectArray<GrahamVector3> orgpoints; - btVector3 averageFaceNormal(0,0,0); + btVector3 averageFaceNormal(0, 0, 0); - for (int i=0;i<coplanarFaceGroup.size();i++) + for (int i = 0; i < coplanarFaceGroup.size(); i++) { -// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + // m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); btFace& face = tmpFaces[coplanarFaceGroup[i]]; - btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]); - averageFaceNormal+=faceNormal; - for (int f=0;f<face.m_indices.size();f++) + btVector3 faceNormal(face.m_plane[0], face.m_plane[1], face.m_plane[2]); + averageFaceNormal += faceNormal; + for (int f = 0; f < face.m_indices.size(); f++) { int orgIndex = face.m_indices[f]; btVector3 pt = m_polyhedron->m_vertices[orgIndex]; - + bool found = false; - for (int i=0;i<orgpoints.size();i++) + for (int i = 0; i < orgpoints.size(); i++) { //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001)) if (orgpoints[i].m_orgIndex == orgIndex) { - found=true; + found = true; break; } } if (!found) - orgpoints.push_back(GrahamVector3(pt,orgIndex)); + orgpoints.push_back(GrahamVector3(pt, orgIndex)); } } - - btFace combinedFace; - for (int i=0;i<4;i++) + for (int i = 0; i < 4; i++) combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i]; btAlignedObjectArray<GrahamVector3> hull; averageFaceNormal.normalize(); - GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal); + GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal); - for (int i=0;i<hull.size();i++) + for (int i = 0; i < hull.size(); i++) { combinedFace.m_indices.push_back(hull[i].m_orgIndex); - for(int k = 0; k < orgpoints.size(); k++) + for (int k = 0; k < orgpoints.size(); k++) { - if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) + if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex) { - orgpoints[k].m_orgIndex = -1; // invalidate... + orgpoints[k].m_orgIndex = -1; // invalidate... break; } } @@ -256,38 +317,41 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa // are there rejected vertices? bool reject_merge = false; - - - for(int i = 0; i < orgpoints.size(); i++) { - if(orgpoints[i].m_orgIndex == -1) - continue; // this is in the hull... + for (int i = 0; i < orgpoints.size(); i++) + { + if (orgpoints[i].m_orgIndex == -1) + continue; // this is in the hull... // this vertex is rejected -- is anybody else using this vertex? - for(int j = 0; j < tmpFaces.size(); j++) { - + for (int j = 0; j < tmpFaces.size(); j++) + { btFace& face = tmpFaces[j]; // is this a face of the current coplanar group? bool is_in_current_group = false; - for(int k = 0; k < coplanarFaceGroup.size(); k++) { - if(coplanarFaceGroup[k] == j) { + for (int k = 0; k < coplanarFaceGroup.size(); k++) + { + if (coplanarFaceGroup[k] == j) + { is_in_current_group = true; break; } } - if(is_in_current_group) // ignore this face... + if (is_in_current_group) // ignore this face... continue; // does this face use this rejected vertex? - for(int v = 0; v < face.m_indices.size(); v++) { - if(face.m_indices[v] == orgpoints[i].m_orgIndex) { + for (int v = 0; v < face.m_indices.size(); v++) + { + if (face.m_indices[v] == orgpoints[i].m_orgIndex) + { // this rejected vertex is used in another face -- reject merge reject_merge = true; break; } } - if(reject_merge) + if (reject_merge) break; } - if(reject_merge) + if (reject_merge) break; } @@ -298,34 +362,30 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa m_polyhedron->m_faces.push_back(combinedFace); } } - if(!did_merge) + if (!did_merge) { - for (int i=0;i<coplanarFaceGroup.size();i++) + for (int i = 0; i < coplanarFaceGroup.size(); i++) { btFace face = tmpFaces[coplanarFaceGroup[i]]; m_polyhedron->m_faces.push_back(face); } + } + } - } - - +#endif //BT_RECONSTRUCT_FACES - } - m_polyhedron->initialize(); return true; } #ifndef MIN - #define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b)) +#define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b)) #endif -btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0) const { - - - btVector3 supVec(0,0,0); + btVector3 supVec(0, 0, 0); #ifndef __SPU__ int i; btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); @@ -334,37 +394,36 @@ btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const b btScalar lenSqr = vec.length2(); if (lenSqr < btScalar(0.0001)) { - vec.setValue(1,0,0); - } else + vec.setValue(1, 0, 0); + } + else { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); vec *= rlen; } btVector3 vtx; btScalar newDot; - for( int k = 0; k < getNumVertices(); k += 128 ) - { - btVector3 temp[128]; - int inner_count = MIN(getNumVertices() - k, 128); - for( i = 0; i < inner_count; i++ ) - getVertex(i,temp[i]); - i = (int) vec.maxDot( temp, inner_count, newDot); + for (int k = 0; k < getNumVertices(); k += 128) + { + btVector3 temp[128]; + int inner_count = MIN(getNumVertices() - k, 128); + for (i = 0; i < inner_count; i++) + getVertex(i, temp[i]); + i = (int)vec.maxDot(temp, inner_count, newDot); if (newDot > maxDot) { maxDot = newDot; supVec = temp[i]; - } - } - -#endif //__SPU__ + } + } + +#endif //__SPU__ return supVec; } - - -void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { #ifndef __SPU__ int i; @@ -372,36 +431,34 @@ void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin( btVector3 vtx; btScalar newDot; - for (i=0;i<numVectors;i++) + for (i = 0; i < numVectors; i++) { supportVerticesOut[i][3] = btScalar(-BT_LARGE_FLOAT); } - for (int j=0;j<numVectors;j++) + for (int j = 0; j < numVectors; j++) { - const btVector3& vec = vectors[j]; - - for( int k = 0; k < getNumVertices(); k += 128 ) - { - btVector3 temp[128]; - int inner_count = MIN(getNumVertices() - k, 128); - for( i = 0; i < inner_count; i++ ) - getVertex(i,temp[i]); - i = (int) vec.maxDot( temp, inner_count, newDot); - if (newDot > supportVerticesOut[j][3]) - { + const btVector3& vec = vectors[j]; + + for (int k = 0; k < getNumVertices(); k += 128) + { + btVector3 temp[128]; + int inner_count = MIN(getNumVertices() - k, 128); + for (i = 0; i < inner_count; i++) + getVertex(i, temp[i]); + i = (int)vec.maxDot(temp, inner_count, newDot); + if (newDot > supportVerticesOut[j][3]) + { supportVerticesOut[j] = temp[i]; supportVerticesOut[j][3] = newDot; - } - } - } + } + } + } -#endif //__SPU__ +#endif //__SPU__ } - - -void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { #ifndef __SPU__ //not yet, return box inertia @@ -410,81 +467,77 @@ void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& ine btTransform ident; ident.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(ident,aabbMin,aabbMax); // This already contains the margin - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); - - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; + btVector3 aabbMin, aabbMax; + getAabb(ident, aabbMin, aabbMax); // This already contains the margin + btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); + + btScalar lx = btScalar(2.) * (halfExtents.x()); + btScalar ly = btScalar(2.) * (halfExtents.y()); + btScalar lz = btScalar(2.) * (halfExtents.z()); + const btScalar x2 = lx * lx; + const btScalar y2 = ly * ly; + const btScalar z2 = lz * lz; const btScalar scaledmass = mass * btScalar(0.08333333); - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); -#endif //__SPU__ + inertia = scaledmass * (btVector3(y2 + z2, x2 + z2, x2 + y2)); +#endif //__SPU__ } - - -void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling) +void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling) { btConvexInternalShape::setLocalScaling(scaling); recalcLocalAabb(); } btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape() -:btPolyhedralConvexShape(), -m_localAabbMin(1,1,1), -m_localAabbMax(-1,-1,-1), -m_isLocalAabbValid(false) + : btPolyhedralConvexShape(), + m_localAabbMin(1, 1, 1), + m_localAabbMax(-1, -1, -1), + m_isLocalAabbValid(false) { } -void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); + getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin()); } -void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() +void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() { m_isLocalAabbValid = true; - - #if 1 + +#if 1 static const btVector3 _directions[] = - { - btVector3( 1., 0., 0.), - btVector3( 0., 1., 0.), - btVector3( 0., 0., 1.), - btVector3( -1., 0., 0.), - btVector3( 0., -1., 0.), - btVector3( 0., 0., -1.) - }; - + { + btVector3(1., 0., 0.), + btVector3(0., 1., 0.), + btVector3(0., 0., 1.), + btVector3(-1., 0., 0.), + btVector3(0., -1., 0.), + btVector3(0., 0., -1.)}; + btVector3 _supporting[] = - { - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.) - }; - + { + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.)}; + batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); - - for ( int i = 0; i < 3; ++i ) + + for (int i = 0; i < 3; ++i) { m_localAabbMax[i] = _supporting[i][i]; m_localAabbMin[i] = _supporting[i + 3][i]; } - #else +#else - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); m_localAabbMax[i] = tmp[i]; @@ -492,9 +545,5 @@ void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() tmp = localGetSupportingVertex(vec); m_localAabbMin[i] = tmp[i]; } - #endif +#endif } - - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 961d001a9dd..b3ffab7a23c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -20,20 +20,15 @@ subject to the following restrictions: #include "btConvexInternalShape.h" class btConvexPolyhedron; - ///The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes. -ATTRIBUTE_ALIGNED16(class) btPolyhedralConvexShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btPolyhedralConvexShape : public btConvexInternalShape { - - protected: - btConvexPolyhedron* m_polyhedron; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - btPolyhedralConvexShape(); @@ -41,76 +36,71 @@ public: ///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges) ///experimental/work-in-progress - virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0); + virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin = 0); - const btConvexPolyhedron* getConvexPolyhedron() const + virtual void setPolyhedralFeatures(btConvexPolyhedron & polyhedron); + + const btConvexPolyhedron* getConvexPolyhedron() const { return m_polyhedron; } //brute force implementations - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - - virtual int getNumVertices() const = 0 ; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; + + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; + + virtual int getNumVertices() const = 0; virtual int getNumEdges() const = 0; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const = 0; - virtual void getVertex(int i,btVector3& vtx) const = 0; - virtual int getNumPlanes() const = 0; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const = 0; -// virtual int getIndex(int i) const = 0 ; - - virtual bool isInside(const btVector3& pt,btScalar tolerance) const = 0; - -}; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const = 0; + virtual void getVertex(int i, btVector3& vtx) const = 0; + virtual int getNumPlanes() const = 0; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const = 0; + // virtual int getIndex(int i) const = 0 ; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const = 0; +}; ///The btPolyhedralConvexAabbCachingShape adds aabb caching to the btPolyhedralConvexShape class btPolyhedralConvexAabbCachingShape : public btPolyhedralConvexShape { + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + bool m_isLocalAabbValid; - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - bool m_isLocalAabbValid; - protected: - - void setCachedLocalAabb (const btVector3& aabbMin, const btVector3& aabbMax) + void setCachedLocalAabb(const btVector3& aabbMin, const btVector3& aabbMax) { m_isLocalAabbValid = true; m_localAabbMin = aabbMin; m_localAabbMax = aabbMax; } - inline void getCachedLocalAabb (btVector3& aabbMin, btVector3& aabbMax) const + inline void getCachedLocalAabb(btVector3& aabbMin, btVector3& aabbMax) const { btAssert(m_isLocalAabbValid); aabbMin = m_localAabbMin; aabbMax = m_localAabbMax; } -public: - +protected: btPolyhedralConvexAabbCachingShape(); - - inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const - { +public: + inline void getNonvirtualAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax, btScalar margin) const + { //lazy evaluation of local aabb btAssert(m_isLocalAabbValid); - btTransformAabb(m_localAabbMin,m_localAabbMax,margin,trans,aabbMin,aabbMax); + btTransformAabb(m_localAabbMin, m_localAabbMax, margin, trans, aabbMin, aabbMax); } - virtual void setLocalScaling(const btVector3& scaling); - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); - void recalcLocalAabb(); + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + void recalcLocalAabb(); }; -#endif //BT_POLYHEDRAL_CONVEX_SHAPE_H +#endif //BT_POLYHEDRAL_CONVEX_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp index 6a337c786c4..f4273199749 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -13,11 +13,10 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btScaledBvhTriangleMeshShape.h" -btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling) -:m_localScaling(localScaling),m_bvhTriMeshShape(childShape) +btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape, const btVector3& localScaling) + : m_localScaling(localScaling), m_bvhTriMeshShape(childShape) { m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; } @@ -26,55 +25,51 @@ btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() { } - class btScaledTriangleCallback : public btTriangleCallback { btTriangleCallback* m_originalCallback; - btVector3 m_localScaling; + btVector3 m_localScaling; public: - - btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling) - :m_originalCallback(originalCallback), - m_localScaling(localScaling) + btScaledTriangleCallback(btTriangleCallback* originalCallback, const btVector3& localScaling) + : m_originalCallback(originalCallback), + m_localScaling(localScaling) { } virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { btVector3 newTriangle[3]; - newTriangle[0] = triangle[0]*m_localScaling; - newTriangle[1] = triangle[1]*m_localScaling; - newTriangle[2] = triangle[2]*m_localScaling; - m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex); + newTriangle[0] = triangle[0] * m_localScaling; + newTriangle[1] = triangle[1] * m_localScaling; + newTriangle[2] = triangle[2] * m_localScaling; + m_originalCallback->processTriangle(&newTriangle[0], partId, triangleIndex); } }; -void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - btScaledTriangleCallback scaledCallback(callback,m_localScaling); - - btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ()); - btVector3 scaledAabbMin,scaledAabbMax; + btScaledTriangleCallback scaledCallback(callback, m_localScaling); + + btVector3 invLocalScaling(1.f / m_localScaling.getX(), 1.f / m_localScaling.getY(), 1.f / m_localScaling.getZ()); + btVector3 scaledAabbMin, scaledAabbMax; ///support negative scaling scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; scaledAabbMin[3] = 0.f; - + scaledAabbMax[0] = m_localScaling.getX() <= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; scaledAabbMax[1] = m_localScaling.getY() <= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; scaledAabbMax[3] = 0.f; - - - m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax); -} + m_bvhTriMeshShape->processAllTriangles(&scaledCallback, scaledAabbMin, scaledAabbMax); +} -void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin(); btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax(); @@ -89,22 +84,21 @@ void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& a localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; - btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); + btVector3 localHalfExtents = btScalar(0.5) * (localAabbMax - localAabbMin); btScalar margin = m_bvhTriMeshShape->getMargin(); - localHalfExtents += btVector3(margin,margin,margin); - btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); - - btMatrix3x3 abs_b = trans.getBasis().absolute(); + localHalfExtents += btVector3(margin, margin, margin); + btVector3 localCenter = btScalar(0.5) * (localAabbMax + localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); aabbMin = center - extent; aabbMax = center + extent; - } -void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } @@ -114,8 +108,8 @@ const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const return m_localScaling; } -void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { ///don't make this a movable object! -// btAssert(0); + // btAssert(0); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h index 39049eaf083..4d6feb61af5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -18,78 +18,69 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" - ///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape. ///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling' -ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btScaledBvhTriangleMeshShape : public btConcaveShape { - - - btVector3 m_localScaling; + btVector3 m_localScaling; - btBvhTriangleMeshShape* m_bvhTriMeshShape; + btBvhTriangleMeshShape* m_bvhTriMeshShape; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling); + btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape * childShape, const btVector3& localScaling); virtual ~btScaledBvhTriangleMeshShape(); - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling); + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - btBvhTriangleMeshShape* getChildShape() + btBvhTriangleMeshShape* getChildShape() { return m_bvhTriMeshShape; } - const btBvhTriangleMeshShape* getChildShape() const + const btBvhTriangleMeshShape* getChildShape() const { return m_bvhTriMeshShape; } //debugging - virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} + virtual const char* getName() const { return "SCALEDBVHTRIANGLEMESH"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btScaledTriangleMeshShapeData +struct btScaledTriangleMeshShapeData { - btTriangleMeshShapeData m_trimeshShapeData; + btTriangleMeshShapeData m_trimeshShapeData; - btVector3FloatData m_localScaling; + btVector3FloatData m_localScaling; }; - -SIMD_FORCE_INLINE int btScaledBvhTriangleMeshShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btScaledBvhTriangleMeshShape::calculateSerializeBufferSize() const { return sizeof(btScaledTriangleMeshShapeData); } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btScaledBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btScaledBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btScaledTriangleMeshShapeData* scaledMeshData = (btScaledTriangleMeshShapeData*) dataBuffer; - m_bvhTriMeshShape->serialize(&scaledMeshData->m_trimeshShapeData,serializer); + btScaledTriangleMeshShapeData* scaledMeshData = (btScaledTriangleMeshShapeData*)dataBuffer; + m_bvhTriMeshShape->serialize(&scaledMeshData->m_trimeshShapeData, serializer); scaledMeshData->m_trimeshShapeData.m_collisionShapeData.m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; m_localScaling.serializeFloat(scaledMeshData->m_localScaling); return "btScaledTriangleMeshShapeData"; } - -#endif //BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H +#endif //BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp new file mode 100644 index 00000000000..4a95dbea4f2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp @@ -0,0 +1,92 @@ +#include "btSdfCollisionShape.h" +#include "btMiniSDF.h" +#include "LinearMath/btAabbUtil2.h" + +struct btSdfCollisionShapeInternalData +{ + btVector3 m_localScaling; + btScalar m_margin; + btMiniSDF m_sdf; + + btSdfCollisionShapeInternalData() + : m_localScaling(1, 1, 1), + m_margin(0) + { + } +}; + +bool btSdfCollisionShape::initializeSDF(const char* sdfData, int sizeInBytes) +{ + bool valid = m_data->m_sdf.load(sdfData, sizeInBytes); + return valid; +} +btSdfCollisionShape::btSdfCollisionShape() +{ + m_shapeType = SDF_SHAPE_PROXYTYPE; + m_data = new btSdfCollisionShapeInternalData(); + + //"E:/develop/bullet3/data/toys/ground_hole64_64_8.cdf");//ground_cube.cdf"); + /*unsigned int field_id=0; + Eigen::Vector3d x (1,10,1); + Eigen::Vector3d gradient; + double dist = m_data->m_sdf.interpolate(field_id, x, &gradient); + printf("dist=%g\n", dist); + */ +} +btSdfCollisionShape::~btSdfCollisionShape() +{ + delete m_data; +} + +void btSdfCollisionShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const +{ + btAssert(m_data->m_sdf.isValid()); + btVector3 localAabbMin = m_data->m_sdf.m_domain.m_min; + btVector3 localAabbMax = m_data->m_sdf.m_domain.m_max; + btScalar margin(0); + btTransformAabb(localAabbMin, localAabbMax, margin, t, aabbMin, aabbMax); +} + +void btSdfCollisionShape::setLocalScaling(const btVector3& scaling) +{ + m_data->m_localScaling = scaling; +} +const btVector3& btSdfCollisionShape::getLocalScaling() const +{ + return m_data->m_localScaling; +} +void btSdfCollisionShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const +{ + inertia.setValue(0, 0, 0); +} +const char* btSdfCollisionShape::getName() const +{ + return "btSdfCollisionShape"; +} +void btSdfCollisionShape::setMargin(btScalar margin) +{ + m_data->m_margin = margin; +} +btScalar btSdfCollisionShape::getMargin() const +{ + return m_data->m_margin; +} + +void btSdfCollisionShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const +{ + //not yet +} + +bool btSdfCollisionShape::queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal) +{ + int field = 0; + btVector3 grad; + double dist; + bool hasResult = m_data->m_sdf.interpolate(field, dist, ptInSDF, &grad); + if (hasResult) + { + normal.setValue(grad[0], grad[1], grad[2]); + distOut = dist; + } + return hasResult; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h new file mode 100644 index 00000000000..3989d6245ed --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h @@ -0,0 +1,29 @@ +#ifndef BT_SDF_COLLISION_SHAPE_H +#define BT_SDF_COLLISION_SHAPE_H + +#include "btConcaveShape.h" + +class btSdfCollisionShape : public btConcaveShape +{ + struct btSdfCollisionShapeInternalData* m_data; + +public: + btSdfCollisionShape(); + virtual ~btSdfCollisionShape(); + + bool initializeSDF(const char* sdfData, int sizeInBytes); + + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const; + virtual const char* getName() const; + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + + virtual void processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const; + + bool queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal); +}; + +#endif //BT_SDF_COLLISION_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp index 3beaf865801..a2c490faf9d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -15,51 +15,48 @@ subject to the following restrictions: //btShapeHull was implemented by John McCutchan. - #include "btShapeHull.h" #include "LinearMath/btConvexHull.h" #define NUM_UNITSPHERE_POINTS 42 +#define NUM_UNITSPHERE_POINTS_HIGHRES 256 -btShapeHull::btShapeHull (const btConvexShape* shape) +btShapeHull::btShapeHull(const btConvexShape* shape) { m_shape = shape; - m_vertices.clear (); + m_vertices.clear(); m_indices.clear(); m_numIndices = 0; } -btShapeHull::~btShapeHull () +btShapeHull::~btShapeHull() { - m_indices.clear(); - m_vertices.clear (); + m_indices.clear(); + m_vertices.clear(); } -bool -btShapeHull::buildHull (btScalar /*margin*/) +bool btShapeHull::buildHull(btScalar /*margin*/, int highres) { - int numSampleDirections = NUM_UNITSPHERE_POINTS; - { - int numPDA = m_shape->getNumPreferredPenetrationDirections(); - if (numPDA) - { - for (int i=0;i<numPDA;i++) - { - btVector3 norm; - m_shape->getPreferredPenetrationDirection(i,norm); - getUnitSpherePoints()[numSampleDirections] = norm; - numSampleDirections++; - } - } - } - - btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + + int numSampleDirections = highres ? NUM_UNITSPHERE_POINTS_HIGHRES : NUM_UNITSPHERE_POINTS; + btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int i; for (i = 0; i < numSampleDirections; i++) { - supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]); + supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]); } + int numPDA = m_shape->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int s = 0; s < numPDA; s++) + { + btVector3 norm; + m_shape->getPreferredPenetrationDirection(s, norm); + supportPoints[i++] = m_shape->localGetSupportingVertex(norm); + numSampleDirections++; + } + } HullDesc hd; hd.mFlags = QF_TRIANGLES; hd.mVcount = static_cast<unsigned int>(numSampleDirections); @@ -69,18 +66,17 @@ btShapeHull::buildHull (btScalar /*margin*/) hd.mVertexStride = sizeof(btVector3); #else hd.mVertices = &supportPoints[0]; - hd.mVertexStride = sizeof (btVector3); + hd.mVertexStride = sizeof(btVector3); #endif HullLibrary hl; HullResult hr; - if (hl.CreateConvexHull (hd, hr) == QE_FAIL) + if (hl.CreateConvexHull(hd, hr) == QE_FAIL) { return false; } - m_vertices.resize (static_cast<int>(hr.mNumOutputVertices)); - + m_vertices.resize(static_cast<int>(hr.mNumOutputVertices)); for (i = 0; i < static_cast<int>(hr.mNumOutputVertices); i++) { @@ -94,77 +90,332 @@ btShapeHull::buildHull (btScalar /*margin*/) } // free temporary hull result that we just copied - hl.ReleaseResult (hr); + hl.ReleaseResult(hr); return true; } -int -btShapeHull::numTriangles () const +int btShapeHull::numTriangles() const { return static_cast<int>(m_numIndices / 3); } -int -btShapeHull::numVertices () const +int btShapeHull::numVertices() const { - return m_vertices.size (); + return m_vertices.size(); } -int -btShapeHull::numIndices () const +int btShapeHull::numIndices() const { return static_cast<int>(m_numIndices); } - -btVector3* btShapeHull::getUnitSpherePoints() +btVector3* btShapeHull::getUnitSpherePoints(int highres) { - static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = - { - btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), - btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), - btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), - btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), - btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), - btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), - btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), - btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), - btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), - btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), - btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), - btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), - btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), - btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), - btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), - btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), - btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), - btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), - btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), - btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), - btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), - btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), - btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), - btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), - btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), - btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), - btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), - btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), - btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), - btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) - }; + static btVector3 sUnitSpherePointsHighres[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] = + { + btVector3(btScalar(0.997604), btScalar(0.067004), btScalar(0.017144)), + btVector3(btScalar(0.984139), btScalar(-0.086784), btScalar(-0.154427)), + btVector3(btScalar(0.971065), btScalar(0.124164), btScalar(-0.203224)), + btVector3(btScalar(0.955844), btScalar(0.291173), btScalar(-0.037704)), + btVector3(btScalar(0.957405), btScalar(0.212238), btScalar(0.195157)), + btVector3(btScalar(0.971650), btScalar(-0.012709), btScalar(0.235561)), + btVector3(btScalar(0.984920), btScalar(-0.161831), btScalar(0.059695)), + btVector3(btScalar(0.946673), btScalar(-0.299288), btScalar(-0.117536)), + btVector3(btScalar(0.922670), btScalar(-0.219186), btScalar(-0.317019)), + btVector3(btScalar(0.928134), btScalar(-0.007265), btScalar(-0.371867)), + btVector3(btScalar(0.875642), btScalar(0.198434), btScalar(-0.439988)), + btVector3(btScalar(0.908035), btScalar(0.325975), btScalar(-0.262562)), + btVector3(btScalar(0.864519), btScalar(0.488706), btScalar(-0.116755)), + btVector3(btScalar(0.893009), btScalar(0.428046), btScalar(0.137185)), + btVector3(btScalar(0.857494), btScalar(0.362137), btScalar(0.364776)), + btVector3(btScalar(0.900815), btScalar(0.132524), btScalar(0.412987)), + btVector3(btScalar(0.934964), btScalar(-0.241739), btScalar(0.259179)), + btVector3(btScalar(0.894570), btScalar(-0.103504), btScalar(0.434263)), + btVector3(btScalar(0.922085), btScalar(-0.376668), btScalar(0.086241)), + btVector3(btScalar(0.862177), btScalar(-0.499154), btScalar(-0.085330)), + btVector3(btScalar(0.861982), btScalar(-0.420218), btScalar(-0.282861)), + btVector3(btScalar(0.818076), btScalar(-0.328256), btScalar(-0.471804)), + btVector3(btScalar(0.762657), btScalar(-0.179329), btScalar(-0.621124)), + btVector3(btScalar(0.826857), btScalar(0.019760), btScalar(-0.561786)), + btVector3(btScalar(0.731434), btScalar(0.206599), btScalar(-0.649817)), + btVector3(btScalar(0.769486), btScalar(0.379052), btScalar(-0.513770)), + btVector3(btScalar(0.796806), btScalar(0.507176), btScalar(-0.328145)), + btVector3(btScalar(0.679722), btScalar(0.684101), btScalar(-0.264123)), + btVector3(btScalar(0.786854), btScalar(0.614886), btScalar(0.050912)), + btVector3(btScalar(0.769486), btScalar(0.571141), btScalar(0.285139)), + btVector3(btScalar(0.707432), btScalar(0.492789), btScalar(0.506288)), + btVector3(btScalar(0.774560), btScalar(0.268037), btScalar(0.572652)), + btVector3(btScalar(0.796220), btScalar(0.031230), btScalar(0.604077)), + btVector3(btScalar(0.837395), btScalar(-0.320285), btScalar(0.442461)), + btVector3(btScalar(0.848127), btScalar(-0.450548), btScalar(0.278307)), + btVector3(btScalar(0.775536), btScalar(-0.206354), btScalar(0.596465)), + btVector3(btScalar(0.816320), btScalar(-0.567007), btScalar(0.109469)), + btVector3(btScalar(0.741191), btScalar(-0.668690), btScalar(-0.056832)), + btVector3(btScalar(0.755632), btScalar(-0.602975), btScalar(-0.254949)), + btVector3(btScalar(0.720311), btScalar(-0.521318), btScalar(-0.457165)), + btVector3(btScalar(0.670746), btScalar(-0.386583), btScalar(-0.632835)), + btVector3(btScalar(0.587031), btScalar(-0.219769), btScalar(-0.778836)), + btVector3(btScalar(0.676015), btScalar(-0.003182), btScalar(-0.736676)), + btVector3(btScalar(0.566932), btScalar(0.186963), btScalar(-0.802064)), + btVector3(btScalar(0.618254), btScalar(0.398105), btScalar(-0.677533)), + btVector3(btScalar(0.653964), btScalar(0.575224), btScalar(-0.490933)), + btVector3(btScalar(0.525367), btScalar(0.743205), btScalar(-0.414028)), + btVector3(btScalar(0.506439), btScalar(0.836528), btScalar(-0.208885)), + btVector3(btScalar(0.651427), btScalar(0.756426), btScalar(-0.056247)), + btVector3(btScalar(0.641670), btScalar(0.745149), btScalar(0.180908)), + btVector3(btScalar(0.602643), btScalar(0.687211), btScalar(0.405180)), + btVector3(btScalar(0.516586), btScalar(0.596999), btScalar(0.613447)), + btVector3(btScalar(0.602252), btScalar(0.387801), btScalar(0.697573)), + btVector3(btScalar(0.646549), btScalar(0.153911), btScalar(0.746956)), + btVector3(btScalar(0.650842), btScalar(-0.087756), btScalar(0.753983)), + btVector3(btScalar(0.740411), btScalar(-0.497404), btScalar(0.451830)), + btVector3(btScalar(0.726946), btScalar(-0.619890), btScalar(0.295093)), + btVector3(btScalar(0.637768), btScalar(-0.313092), btScalar(0.703624)), + btVector3(btScalar(0.678942), btScalar(-0.722934), btScalar(0.126645)), + btVector3(btScalar(0.489072), btScalar(-0.867195), btScalar(-0.092942)), + btVector3(btScalar(0.622742), btScalar(-0.757541), btScalar(-0.194636)), + btVector3(btScalar(0.596788), btScalar(-0.693576), btScalar(-0.403098)), + btVector3(btScalar(0.550150), btScalar(-0.582172), btScalar(-0.598287)), + btVector3(btScalar(0.474436), btScalar(-0.429745), btScalar(-0.768101)), + btVector3(btScalar(0.372574), btScalar(-0.246016), btScalar(-0.894583)), + btVector3(btScalar(0.480095), btScalar(-0.026513), btScalar(-0.876626)), + btVector3(btScalar(0.352474), btScalar(0.177242), btScalar(-0.918787)), + btVector3(btScalar(0.441848), btScalar(0.374386), btScalar(-0.814946)), + btVector3(btScalar(0.492389), btScalar(0.582223), btScalar(-0.646693)), + btVector3(btScalar(0.343498), btScalar(0.866080), btScalar(-0.362693)), + btVector3(btScalar(0.362036), btScalar(0.745149), btScalar(-0.559639)), + btVector3(btScalar(0.334131), btScalar(0.937044), btScalar(-0.099774)), + btVector3(btScalar(0.486925), btScalar(0.871718), btScalar(0.052473)), + btVector3(btScalar(0.452776), btScalar(0.845665), btScalar(0.281820)), + btVector3(btScalar(0.399503), btScalar(0.771785), btScalar(0.494576)), + btVector3(btScalar(0.296469), btScalar(0.673018), btScalar(0.677469)), + btVector3(btScalar(0.392088), btScalar(0.479179), btScalar(0.785213)), + btVector3(btScalar(0.452190), btScalar(0.252094), btScalar(0.855286)), + btVector3(btScalar(0.478339), btScalar(0.013149), btScalar(0.877928)), + btVector3(btScalar(0.481656), btScalar(-0.219380), btScalar(0.848259)), + btVector3(btScalar(0.615327), btScalar(-0.494293), btScalar(0.613837)), + btVector3(btScalar(0.594642), btScalar(-0.650414), btScalar(0.472325)), + btVector3(btScalar(0.562249), btScalar(-0.771345), btScalar(0.297631)), + btVector3(btScalar(0.467411), btScalar(-0.437133), btScalar(0.768231)), + btVector3(btScalar(0.519513), btScalar(-0.847947), btScalar(0.103808)), + btVector3(btScalar(0.297640), btScalar(-0.938159), btScalar(-0.176288)), + btVector3(btScalar(0.446727), btScalar(-0.838615), btScalar(-0.311359)), + btVector3(btScalar(0.331790), btScalar(-0.942437), btScalar(0.040762)), + btVector3(btScalar(0.413358), btScalar(-0.748403), btScalar(-0.518259)), + btVector3(btScalar(0.347596), btScalar(-0.621640), btScalar(-0.701737)), + btVector3(btScalar(0.249831), btScalar(-0.456186), btScalar(-0.853984)), + btVector3(btScalar(0.131772), btScalar(-0.262931), btScalar(-0.955678)), + btVector3(btScalar(0.247099), btScalar(-0.042261), btScalar(-0.967975)), + btVector3(btScalar(0.113624), btScalar(0.165965), btScalar(-0.979491)), + btVector3(btScalar(0.217438), btScalar(0.374580), btScalar(-0.901220)), + btVector3(btScalar(0.307983), btScalar(0.554615), btScalar(-0.772786)), + btVector3(btScalar(0.166702), btScalar(0.953181), btScalar(-0.252021)), + btVector3(btScalar(0.172751), btScalar(0.844499), btScalar(-0.506743)), + btVector3(btScalar(0.177630), btScalar(0.711125), btScalar(-0.679876)), + btVector3(btScalar(0.120064), btScalar(0.992260), btScalar(-0.030482)), + btVector3(btScalar(0.289640), btScalar(0.949098), btScalar(0.122546)), + btVector3(btScalar(0.239879), btScalar(0.909047), btScalar(0.340377)), + btVector3(btScalar(0.181142), btScalar(0.821363), btScalar(0.540641)), + btVector3(btScalar(0.066986), btScalar(0.719097), btScalar(0.691327)), + btVector3(btScalar(0.156750), btScalar(0.545478), btScalar(0.823079)), + btVector3(btScalar(0.236172), btScalar(0.342306), btScalar(0.909353)), + btVector3(btScalar(0.277541), btScalar(0.112693), btScalar(0.953856)), + btVector3(btScalar(0.295299), btScalar(-0.121974), btScalar(0.947415)), + btVector3(btScalar(0.287883), btScalar(-0.349254), btScalar(0.891591)), + btVector3(btScalar(0.437165), btScalar(-0.634666), btScalar(0.636869)), + btVector3(btScalar(0.407113), btScalar(-0.784954), btScalar(0.466664)), + btVector3(btScalar(0.375111), btScalar(-0.888193), btScalar(0.264839)), + btVector3(btScalar(0.275394), btScalar(-0.560591), btScalar(0.780723)), + btVector3(btScalar(0.122015), btScalar(-0.992209), btScalar(-0.024821)), + btVector3(btScalar(0.087866), btScalar(-0.966156), btScalar(-0.241676)), + btVector3(btScalar(0.239489), btScalar(-0.885665), btScalar(-0.397437)), + btVector3(btScalar(0.167287), btScalar(-0.965184), btScalar(0.200817)), + btVector3(btScalar(0.201632), btScalar(-0.776789), btScalar(-0.596335)), + btVector3(btScalar(0.122015), btScalar(-0.637971), btScalar(-0.760098)), + btVector3(btScalar(0.008054), btScalar(-0.464741), btScalar(-0.885214)), + btVector3(btScalar(-0.116054), btScalar(-0.271096), btScalar(-0.955482)), + btVector3(btScalar(-0.000727), btScalar(-0.056065), btScalar(-0.998424)), + btVector3(btScalar(-0.134007), btScalar(0.152939), btScalar(-0.978905)), + btVector3(btScalar(-0.025900), btScalar(0.366026), btScalar(-0.930108)), + btVector3(btScalar(0.081231), btScalar(0.557337), btScalar(-0.826072)), + btVector3(btScalar(-0.002874), btScalar(0.917213), btScalar(-0.398023)), + btVector3(btScalar(-0.050683), btScalar(0.981761), btScalar(-0.182534)), + btVector3(btScalar(-0.040536), btScalar(0.710153), btScalar(-0.702713)), + btVector3(btScalar(-0.139081), btScalar(0.827973), btScalar(-0.543048)), + btVector3(btScalar(-0.101029), btScalar(0.994010), btScalar(0.041152)), + btVector3(btScalar(0.069328), btScalar(0.978067), btScalar(0.196133)), + btVector3(btScalar(0.023860), btScalar(0.911380), btScalar(0.410645)), + btVector3(btScalar(-0.153521), btScalar(0.736789), btScalar(0.658145)), + btVector3(btScalar(-0.070002), btScalar(0.591750), btScalar(0.802780)), + btVector3(btScalar(0.002590), btScalar(0.312948), btScalar(0.949562)), + btVector3(btScalar(0.090988), btScalar(-0.020680), btScalar(0.995627)), + btVector3(btScalar(0.088842), btScalar(-0.250099), btScalar(0.964006)), + btVector3(btScalar(0.083378), btScalar(-0.470185), btScalar(0.878318)), + btVector3(btScalar(0.240074), btScalar(-0.749764), btScalar(0.616374)), + btVector3(btScalar(0.210803), btScalar(-0.885860), btScalar(0.412987)), + btVector3(btScalar(0.077524), btScalar(-0.660524), btScalar(0.746565)), + btVector3(btScalar(-0.096736), btScalar(-0.990070), btScalar(-0.100945)), + btVector3(btScalar(-0.052634), btScalar(-0.990264), btScalar(0.127426)), + btVector3(btScalar(-0.106102), btScalar(-0.938354), btScalar(-0.328340)), + btVector3(btScalar(0.013323), btScalar(-0.863112), btScalar(-0.504596)), + btVector3(btScalar(-0.002093), btScalar(-0.936993), btScalar(0.349161)), + btVector3(btScalar(-0.106297), btScalar(-0.636610), btScalar(-0.763612)), + btVector3(btScalar(-0.229430), btScalar(-0.463769), btScalar(-0.855546)), + btVector3(btScalar(-0.245236), btScalar(-0.066175), btScalar(-0.966999)), + btVector3(btScalar(-0.351587), btScalar(-0.270513), btScalar(-0.896145)), + btVector3(btScalar(-0.370906), btScalar(0.133108), btScalar(-0.918982)), + btVector3(btScalar(-0.264360), btScalar(0.346000), btScalar(-0.900049)), + btVector3(btScalar(-0.151375), btScalar(0.543728), btScalar(-0.825291)), + btVector3(btScalar(-0.218697), btScalar(0.912741), btScalar(-0.344346)), + btVector3(btScalar(-0.274507), btScalar(0.953764), btScalar(-0.121635)), + btVector3(btScalar(-0.259677), btScalar(0.692266), btScalar(-0.673044)), + btVector3(btScalar(-0.350416), btScalar(0.798810), btScalar(-0.488786)), + btVector3(btScalar(-0.320170), btScalar(0.941127), btScalar(0.108297)), + btVector3(btScalar(-0.147667), btScalar(0.952792), btScalar(0.265034)), + btVector3(btScalar(-0.188061), btScalar(0.860636), btScalar(0.472910)), + btVector3(btScalar(-0.370906), btScalar(0.739900), btScalar(0.560941)), + btVector3(btScalar(-0.297143), btScalar(0.585334), btScalar(0.754178)), + btVector3(btScalar(-0.189622), btScalar(0.428241), btScalar(0.883393)), + btVector3(btScalar(-0.091272), btScalar(0.098695), btScalar(0.990747)), + btVector3(btScalar(-0.256945), btScalar(0.228375), btScalar(0.938827)), + btVector3(btScalar(-0.111761), btScalar(-0.133251), btScalar(0.984696)), + btVector3(btScalar(-0.118006), btScalar(-0.356253), btScalar(0.926725)), + btVector3(btScalar(-0.119372), btScalar(-0.563896), btScalar(0.817029)), + btVector3(btScalar(0.041228), btScalar(-0.833949), btScalar(0.550010)), + btVector3(btScalar(-0.121909), btScalar(-0.736543), btScalar(0.665172)), + btVector3(btScalar(-0.307681), btScalar(-0.931160), btScalar(-0.195026)), + btVector3(btScalar(-0.283679), btScalar(-0.957990), btScalar(0.041348)), + btVector3(btScalar(-0.227284), btScalar(-0.935243), btScalar(0.270890)), + btVector3(btScalar(-0.293436), btScalar(-0.858252), btScalar(-0.420860)), + btVector3(btScalar(-0.175767), btScalar(-0.780677), btScalar(-0.599262)), + btVector3(btScalar(-0.170108), btScalar(-0.858835), btScalar(0.482865)), + btVector3(btScalar(-0.332854), btScalar(-0.635055), btScalar(-0.696857)), + btVector3(btScalar(-0.447791), btScalar(-0.445299), btScalar(-0.775128)), + btVector3(btScalar(-0.470622), btScalar(-0.074146), btScalar(-0.879164)), + btVector3(btScalar(-0.639417), btScalar(-0.340505), btScalar(-0.689049)), + btVector3(btScalar(-0.598438), btScalar(0.104722), btScalar(-0.794256)), + btVector3(btScalar(-0.488575), btScalar(0.307699), btScalar(-0.816313)), + btVector3(btScalar(-0.379882), btScalar(0.513592), btScalar(-0.769077)), + btVector3(btScalar(-0.425740), btScalar(0.862775), btScalar(-0.272516)), + btVector3(btScalar(-0.480769), btScalar(0.875412), btScalar(-0.048439)), + btVector3(btScalar(-0.467890), btScalar(0.648716), btScalar(-0.600043)), + btVector3(btScalar(-0.543799), btScalar(0.730956), btScalar(-0.411881)), + btVector3(btScalar(-0.516284), btScalar(0.838277), btScalar(0.174076)), + btVector3(btScalar(-0.353343), btScalar(0.876384), btScalar(0.326519)), + btVector3(btScalar(-0.572875), btScalar(0.614497), btScalar(0.542007)), + btVector3(btScalar(-0.503600), btScalar(0.497261), btScalar(0.706161)), + btVector3(btScalar(-0.530920), btScalar(0.754870), btScalar(0.384685)), + btVector3(btScalar(-0.395884), btScalar(0.366414), btScalar(0.841818)), + btVector3(btScalar(-0.300656), btScalar(0.001678), btScalar(0.953661)), + btVector3(btScalar(-0.461060), btScalar(0.146912), btScalar(0.875000)), + btVector3(btScalar(-0.315486), btScalar(-0.232212), btScalar(0.919893)), + btVector3(btScalar(-0.323682), btScalar(-0.449187), btScalar(0.832644)), + btVector3(btScalar(-0.318999), btScalar(-0.639527), btScalar(0.699134)), + btVector3(btScalar(-0.496771), btScalar(-0.866029), btScalar(-0.055271)), + btVector3(btScalar(-0.496771), btScalar(-0.816257), btScalar(-0.294377)), + btVector3(btScalar(-0.456377), btScalar(-0.869528), btScalar(0.188130)), + btVector3(btScalar(-0.380858), btScalar(-0.827144), btScalar(0.412792)), + btVector3(btScalar(-0.449352), btScalar(-0.727405), btScalar(-0.518259)), + btVector3(btScalar(-0.570533), btScalar(-0.551064), btScalar(-0.608632)), + btVector3(btScalar(-0.656394), btScalar(-0.118280), btScalar(-0.744874)), + btVector3(btScalar(-0.756696), btScalar(-0.438105), btScalar(-0.484882)), + btVector3(btScalar(-0.801773), btScalar(-0.204798), btScalar(-0.561005)), + btVector3(btScalar(-0.785186), btScalar(0.038618), btScalar(-0.617805)), + btVector3(btScalar(-0.709082), btScalar(0.262399), btScalar(-0.654306)), + btVector3(btScalar(-0.583412), btScalar(0.462265), btScalar(-0.667383)), + btVector3(btScalar(-0.616001), btScalar(0.761286), btScalar(-0.201272)), + btVector3(btScalar(-0.660687), btScalar(0.750204), btScalar(0.020072)), + btVector3(btScalar(-0.744987), btScalar(0.435823), btScalar(-0.504791)), + btVector3(btScalar(-0.713765), btScalar(0.605554), btScalar(-0.351373)), + btVector3(btScalar(-0.686251), btScalar(0.687600), btScalar(0.236927)), + btVector3(btScalar(-0.680201), btScalar(0.429407), btScalar(0.593732)), + btVector3(btScalar(-0.733474), btScalar(0.546450), btScalar(0.403814)), + btVector3(btScalar(-0.591023), btScalar(0.292923), btScalar(0.751445)), + btVector3(btScalar(-0.500283), btScalar(-0.080757), btScalar(0.861922)), + btVector3(btScalar(-0.643710), btScalar(0.070115), btScalar(0.761985)), + btVector3(btScalar(-0.506332), btScalar(-0.308425), btScalar(0.805122)), + btVector3(btScalar(-0.503015), btScalar(-0.509847), btScalar(0.697573)), + btVector3(btScalar(-0.482525), btScalar(-0.682105), btScalar(0.549229)), + btVector3(btScalar(-0.680396), btScalar(-0.716323), btScalar(-0.153451)), + btVector3(btScalar(-0.658346), btScalar(-0.746264), btScalar(0.097562)), + btVector3(btScalar(-0.653272), btScalar(-0.646915), btScalar(-0.392948)), + btVector3(btScalar(-0.590828), btScalar(-0.732655), btScalar(0.337645)), + btVector3(btScalar(-0.819140), btScalar(-0.518013), btScalar(-0.246166)), + btVector3(btScalar(-0.900513), btScalar(-0.282178), btScalar(-0.330487)), + btVector3(btScalar(-0.914953), btScalar(-0.028652), btScalar(-0.402122)), + btVector3(btScalar(-0.859924), btScalar(0.220209), btScalar(-0.459898)), + btVector3(btScalar(-0.777185), btScalar(0.613720), btScalar(-0.137836)), + btVector3(btScalar(-0.805285), btScalar(0.586889), btScalar(0.082728)), + btVector3(btScalar(-0.872413), btScalar(0.406077), btScalar(-0.271735)), + btVector3(btScalar(-0.859339), btScalar(0.448072), btScalar(0.246101)), + btVector3(btScalar(-0.757671), btScalar(0.216320), btScalar(0.615594)), + btVector3(btScalar(-0.826165), btScalar(0.348139), btScalar(0.442851)), + btVector3(btScalar(-0.671810), btScalar(-0.162803), btScalar(0.722557)), + btVector3(btScalar(-0.796504), btScalar(-0.004543), btScalar(0.604468)), + btVector3(btScalar(-0.676298), btScalar(-0.378223), btScalar(0.631794)), + btVector3(btScalar(-0.668883), btScalar(-0.558258), btScalar(0.490673)), + btVector3(btScalar(-0.821287), btScalar(-0.570118), btScalar(0.006994)), + btVector3(btScalar(-0.767428), btScalar(-0.587810), btScalar(0.255470)), + btVector3(btScalar(-0.933296), btScalar(-0.349837), btScalar(-0.079865)), + btVector3(btScalar(-0.982667), btScalar(-0.100393), btScalar(-0.155208)), + btVector3(btScalar(-0.961396), btScalar(0.160910), btScalar(-0.222938)), + btVector3(btScalar(-0.934858), btScalar(0.354555), btScalar(-0.006864)), + btVector3(btScalar(-0.941687), btScalar(0.229736), btScalar(0.245711)), + btVector3(btScalar(-0.884317), btScalar(0.131552), btScalar(0.447536)), + btVector3(btScalar(-0.810359), btScalar(-0.219769), btScalar(0.542788)), + btVector3(btScalar(-0.915929), btScalar(-0.210048), btScalar(0.341743)), + btVector3(btScalar(-0.816799), btScalar(-0.407192), btScalar(0.408303)), + btVector3(btScalar(-0.903050), btScalar(-0.392416), btScalar(0.174076)), + btVector3(btScalar(-0.980325), btScalar(-0.170969), btScalar(0.096586)), + btVector3(btScalar(-0.995936), btScalar(0.084891), btScalar(0.029441)), + btVector3(btScalar(-0.960031), btScalar(0.002650), btScalar(0.279283)), + }; + static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] = + { + btVector3(btScalar(0.000000), btScalar(-0.000000), btScalar(-1.000000)), + btVector3(btScalar(0.723608), btScalar(-0.525725), btScalar(-0.447219)), + btVector3(btScalar(-0.276388), btScalar(-0.850649), btScalar(-0.447219)), + btVector3(btScalar(-0.894426), btScalar(-0.000000), btScalar(-0.447216)), + btVector3(btScalar(-0.276388), btScalar(0.850649), btScalar(-0.447220)), + btVector3(btScalar(0.723608), btScalar(0.525725), btScalar(-0.447219)), + btVector3(btScalar(0.276388), btScalar(-0.850649), btScalar(0.447220)), + btVector3(btScalar(-0.723608), btScalar(-0.525725), btScalar(0.447219)), + btVector3(btScalar(-0.723608), btScalar(0.525725), btScalar(0.447219)), + btVector3(btScalar(0.276388), btScalar(0.850649), btScalar(0.447219)), + btVector3(btScalar(0.894426), btScalar(0.000000), btScalar(0.447216)), + btVector3(btScalar(-0.000000), btScalar(0.000000), btScalar(1.000000)), + btVector3(btScalar(0.425323), btScalar(-0.309011), btScalar(-0.850654)), + btVector3(btScalar(-0.162456), btScalar(-0.499995), btScalar(-0.850654)), + btVector3(btScalar(0.262869), btScalar(-0.809012), btScalar(-0.525738)), + btVector3(btScalar(0.425323), btScalar(0.309011), btScalar(-0.850654)), + btVector3(btScalar(0.850648), btScalar(-0.000000), btScalar(-0.525736)), + btVector3(btScalar(-0.525730), btScalar(-0.000000), btScalar(-0.850652)), + btVector3(btScalar(-0.688190), btScalar(-0.499997), btScalar(-0.525736)), + btVector3(btScalar(-0.162456), btScalar(0.499995), btScalar(-0.850654)), + btVector3(btScalar(-0.688190), btScalar(0.499997), btScalar(-0.525736)), + btVector3(btScalar(0.262869), btScalar(0.809012), btScalar(-0.525738)), + btVector3(btScalar(0.951058), btScalar(0.309013), btScalar(0.000000)), + btVector3(btScalar(0.951058), btScalar(-0.309013), btScalar(0.000000)), + btVector3(btScalar(0.587786), btScalar(-0.809017), btScalar(0.000000)), + btVector3(btScalar(0.000000), btScalar(-1.000000), btScalar(0.000000)), + btVector3(btScalar(-0.587786), btScalar(-0.809017), btScalar(0.000000)), + btVector3(btScalar(-0.951058), btScalar(-0.309013), btScalar(-0.000000)), + btVector3(btScalar(-0.951058), btScalar(0.309013), btScalar(-0.000000)), + btVector3(btScalar(-0.587786), btScalar(0.809017), btScalar(-0.000000)), + btVector3(btScalar(-0.000000), btScalar(1.000000), btScalar(-0.000000)), + btVector3(btScalar(0.587786), btScalar(0.809017), btScalar(-0.000000)), + btVector3(btScalar(0.688190), btScalar(-0.499997), btScalar(0.525736)), + btVector3(btScalar(-0.262869), btScalar(-0.809012), btScalar(0.525738)), + btVector3(btScalar(-0.850648), btScalar(0.000000), btScalar(0.525736)), + btVector3(btScalar(-0.262869), btScalar(0.809012), btScalar(0.525738)), + btVector3(btScalar(0.688190), btScalar(0.499997), btScalar(0.525736)), + btVector3(btScalar(0.525730), btScalar(0.000000), btScalar(0.850652)), + btVector3(btScalar(0.162456), btScalar(-0.499995), btScalar(0.850654)), + btVector3(btScalar(-0.425323), btScalar(-0.309011), btScalar(0.850654)), + btVector3(btScalar(-0.425323), btScalar(0.309011), btScalar(0.850654)), + btVector3(btScalar(0.162456), btScalar(0.499995), btScalar(0.850654))}; + if (highres) + return sUnitSpherePointsHighres; return sUnitSpherePoints; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h index e959f198b69..54439f9ca25 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h @@ -21,32 +21,31 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" - ///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices. ///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object. ///It approximates the convex hull using the supporting vertex of 42 directions. -ATTRIBUTE_ALIGNED16(class) btShapeHull +ATTRIBUTE_ALIGNED16(class) +btShapeHull { protected: - btAlignedObjectArray<btVector3> m_vertices; btAlignedObjectArray<unsigned int> m_indices; unsigned int m_numIndices; const btConvexShape* m_shape; - static btVector3* getUnitSpherePoints(); + static btVector3* getUnitSpherePoints(int highres = 0); public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btShapeHull (const btConvexShape* shape); - ~btShapeHull (); - bool buildHull (btScalar margin); + btShapeHull(const btConvexShape* shape); + ~btShapeHull(); + + bool buildHull(btScalar margin, int highres = 0); - int numTriangles () const; - int numVertices () const; - int numIndices () const; + int numTriangles() const; + int numVertices() const; + int numIndices() const; const btVector3* getVertexPointer() const { @@ -58,4 +57,4 @@ public: } }; -#endif //BT_SHAPE_HULL_H +#endif //BT_SHAPE_HULL_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp index b9a736c0fdd..027db2e1046 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp @@ -18,54 +18,48 @@ subject to the following restrictions: #include "LinearMath/btQuaternion.h" -btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { (void)vec; - return btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); + return btVector3(btScalar(0.), btScalar(0.), btScalar(0.)); } -void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { (void)vectors; - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { - supportVerticesOut[i].setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + supportVerticesOut[i].setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } } - -btVector3 btSphereShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btSphereShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supVertex; supVertex = localGetSupportingVertexWithoutMargin(vec); btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + if (vecnorm.length2() < (SIMD_EPSILON * SIMD_EPSILON)) { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } + vecnorm.setValue(btScalar(-1.), btScalar(-1.), btScalar(-1.)); + } vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; + supVertex += getMargin() * vecnorm; return supVertex; } - //broken due to scaling -void btSphereShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btSphereShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { const btVector3& center = t.getOrigin(); - btVector3 extent(getMargin(),getMargin(),getMargin()); + btVector3 extent(getMargin(), getMargin(), getMargin()); aabbMin = center - extent; aabbMax = center + extent; } - - -void btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btSphereShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { - btScalar elem = btScalar(0.4) * mass * getMargin()*getMargin(); - inertia.setValue(elem,elem,elem); - + btScalar elem = btScalar(0.4) * mass * getMargin() * getMargin(); + inertia.setValue(elem, elem, elem); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h index b192efeeb8d..75e4fd8e189 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h @@ -16,58 +16,56 @@ subject to the following restrictions: #define BT_SPHERE_MINKOWSKI_H #include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///The btSphereShape implements an implicit sphere, centered around a local origin with radius. -ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexInternalShape +ATTRIBUTE_ALIGNED16(class) +btSphereShape : public btConvexInternalShape { - public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btSphereShape (btScalar radius) : btConvexInternalShape () + btSphereShape(btScalar radius) : btConvexInternalShape() { m_shapeType = SPHERE_SHAPE_PROXYTYPE; + m_localScaling.setValue(1.0, 1.0, 1.0); + m_implicitShapeDimensions.setZero(); m_implicitShapeDimensions.setX(radius); m_collisionMargin = radius; + m_padding = 0; } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + //notice that the vectors should be unit length + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - btScalar getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();} + btScalar getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX(); } - void setUnscaledRadius(btScalar radius) + void setUnscaledRadius(btScalar radius) { m_implicitShapeDimensions.setX(radius); btConvexInternalShape::setMargin(radius); } //debugging - virtual const char* getName()const {return "SPHERE";} + virtual const char* getName() const { return "SPHERE"; } - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { btConvexInternalShape::setMargin(margin); } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { //to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case //this means, non-uniform scaling is not supported anymore return getRadius(); } - - }; - -#endif //BT_SPHERE_MINKOWSKI_H +#endif //BT_SPHERE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp index d17141e3f20..9238c919d51 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -17,24 +17,18 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" - -btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant) -: btConcaveShape (), m_planeNormal(planeNormal.normalized()), -m_planeConstant(planeConstant), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) +btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal, btScalar planeConstant) + : btConcaveShape(), m_planeNormal(planeNormal.normalized()), m_planeConstant(planeConstant), m_localScaling(btScalar(1.), btScalar(1.), btScalar(1.)) { m_shapeType = STATIC_PLANE_PROXYTYPE; // btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) ); } - btStaticPlaneShape::~btStaticPlaneShape() { } - - -void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btStaticPlaneShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { (void)t; /* @@ -47,57 +41,49 @@ void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVecto aabbMax.setMax(center - infvec*m_planeNormal); */ - aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - + aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); } - - - -void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5); btScalar radius = halfExtents.length(); btVector3 center = (aabbMax + aabbMin) * btScalar(0.5); - + //this is where the triangles are generated, given AABB and plane equation (normal/constant) - btVector3 tangentDir0,tangentDir1; + btVector3 tangentDir0, tangentDir1; //tangentDir0/tangentDir1 can be precalculated - btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1); + btPlaneSpace1(m_planeNormal, tangentDir0, tangentDir1); - btVector3 supVertex0,supVertex1; + btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant) * m_planeNormal; - btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal; - btVector3 triangle[3]; - triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius; - triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius; - triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius; - - callback->processTriangle(triangle,0,0); + triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; + triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius; + triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; - triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius; - triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius; - triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius; + callback->processTriangle(triangle, 0, 0); - callback->processTriangle(triangle,0,1); + triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius; + triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius; + triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius; + callback->processTriangle(triangle, 0, 1); } -void btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btStaticPlaneShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { (void)mass; //moving concave objects not supported - - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } -void btStaticPlaneShape::setLocalScaling(const btVector3& scaling) +void btStaticPlaneShape::setLocalScaling(const btVector3& scaling) { m_localScaling = scaling; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index e6e32883959..1cda8bbc75b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -18,88 +18,86 @@ subject to the following restrictions: #include "btConcaveShape.h" - ///The btStaticPlaneShape simulates an infinite non-moving (static) collision plane. -ATTRIBUTE_ALIGNED16(class) btStaticPlaneShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btStaticPlaneShape : public btConcaveShape { protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - - btVector3 m_planeNormal; - btScalar m_planeConstant; - btVector3 m_localScaling; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + + btVector3 m_planeNormal; + btScalar m_planeConstant; + btVector3 m_localScaling; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant); + btStaticPlaneShape(const btVector3& planeNormal, btScalar planeConstant); virtual ~btStaticPlaneShape(); + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; - - const btVector3& getPlaneNormal() const + + const btVector3& getPlaneNormal() const { - return m_planeNormal; + return m_planeNormal; } - const btScalar& getPlaneConstant() const + const btScalar& getPlaneConstant() const { - return m_planeConstant; + return m_planeConstant; } //debugging - virtual const char* getName()const {return "STATICPLANE";} + virtual const char* getName() const { return "STATICPLANE"; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btStaticPlaneShapeData +struct btStaticPlaneShapeData { - btCollisionShapeData m_collisionShapeData; + btCollisionShapeData m_collisionShapeData; - btVector3FloatData m_localScaling; - btVector3FloatData m_planeNormal; - float m_planeConstant; - char m_pad[4]; + btVector3FloatData m_localScaling; + btVector3FloatData m_planeNormal; + float m_planeConstant; + char m_pad[4]; }; - -SIMD_FORCE_INLINE int btStaticPlaneShape::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btStaticPlaneShape::calculateSerializeBufferSize() const { return sizeof(btStaticPlaneShapeData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, btSerializer* serializer) const { - btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*) dataBuffer; - btCollisionShape::serialize(&planeData->m_collisionShapeData,serializer); + btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)dataBuffer; + btCollisionShape::serialize(&planeData->m_collisionShapeData, serializer); m_localScaling.serializeFloat(planeData->m_localScaling); m_planeNormal.serializeFloat(planeData->m_planeNormal); planeData->m_planeConstant = float(m_planeConstant); - - return "btStaticPlaneShapeData"; -} - - -#endif //BT_STATIC_PLANE_SHAPE_H + // Fill padding with zeros to appease msan. + planeData->m_pad[0] = 0; + planeData->m_pad[1] = 0; + planeData->m_pad[2] = 0; + planeData->m_pad[3] = 0; + return "btStaticPlaneShapeData"; +} +#endif //BT_STATIC_PLANE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp index b3d449676b9..eb288e99c91 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp @@ -18,32 +18,30 @@ subject to the following restrictions: btStridingMeshInterface::~btStridingMeshInterface() { - } - -void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { (void)aabbMin; (void)aabbMax; int numtotalphysicsverts = 0; - int part,graphicssubparts = getNumSubParts(); - const unsigned char * vertexbase; - const unsigned char * indexbase; + int part, graphicssubparts = getNumSubParts(); + const unsigned char* vertexbase; + const unsigned char* indexbase; int indexstride; PHY_ScalarType type; PHY_ScalarType gfxindextype; - int stride,numverts,numtriangles; + int stride, numverts, numtriangles; int gfxindex; btVector3 triangle[3]; btVector3 meshScaling = getScaling(); ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype - for (part=0;part<graphicssubparts ;part++) + for (part = 0; part < graphicssubparts; part++) { - getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part); - numtotalphysicsverts+=numtriangles*3; //upper bound + getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part); + numtotalphysicsverts += numtriangles * 3; //upper bound ///unlike that developers want to pass in double-precision meshes in single-precision Bullet build ///so disable this feature by default @@ -51,143 +49,141 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde switch (type) { - case PHY_FLOAT: - { - - float* graphicsbase; - - switch (gfxindextype) - { - case PHY_INTEGER: - { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) - { - unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); - graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); - } - break; - } - case PHY_SHORT: - { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) - { - unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); - graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); - } - break; - } - case PHY_UCHAR: - { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) - { - unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); - graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); - } - break; - } - default: - btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); - } - break; - } - - case PHY_DOUBLE: + case PHY_FLOAT: + { + float* graphicsbase; + + switch (gfxindextype) + { + case PHY_INTEGER: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + case PHY_SHORT: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + case PHY_UCHAR: + { + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) + { + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); + graphicsbase = (float*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (float*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue(graphicsbase[0] * meshScaling.getX(), graphicsbase[1] * meshScaling.getY(), graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + default: + btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + } + break; + } + + case PHY_DOUBLE: { double* graphicsbase; switch (gfxindextype) { - case PHY_INTEGER: + case PHY_INTEGER: { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); - graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); + unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } - case PHY_SHORT: + case PHY_SHORT: { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); - graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } - case PHY_UCHAR: + case PHY_UCHAR: { - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); - graphicsbase = (double*)(vertexbase+tri_indices[0]*stride); - triangle[0].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(),(btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[1]*stride); - triangle[1].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - graphicsbase = (double*)(vertexbase+tri_indices[2]*stride); - triangle[2].setValue((btScalar)graphicsbase[0]*meshScaling.getX(),(btScalar)graphicsbase[1]*meshScaling.getY(), (btScalar)graphicsbase[2]*meshScaling.getZ()); - callback->internalProcessTriangleIndex(triangle,part,gfxindex); + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); + graphicsbase = (double*)(vertexbase + tri_indices[0] * stride); + triangle[0].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[1] * stride); + triangle[1].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + graphicsbase = (double*)(vertexbase + tri_indices[2] * stride); + triangle[2].setValue((btScalar)graphicsbase[0] * meshScaling.getX(), (btScalar)graphicsbase[1] * meshScaling.getY(), (btScalar)graphicsbase[2] * meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle, part, gfxindex); } break; } - default: - btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + default: + btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); } break; } - default: - btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); + default: + btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); } unLockReadOnlyVertexBase(part); } } -void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax) +void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin, btVector3& aabbMax) { - - struct AabbCalculationCallback : public btInternalTriangleIndexCallback + struct AabbCalculationCallback : public btInternalTriangleIndexCallback { - btVector3 m_aabbMin; - btVector3 m_aabbMax; + btVector3 m_aabbMin; + btVector3 m_aabbMax; AabbCalculationCallback() { - m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { (void)partId; (void)triangleIndex; @@ -202,21 +198,19 @@ void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVecto }; //first calculate the total aabb for all triangles - AabbCalculationCallback aabbCallback; - aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax); + AabbCalculationCallback aabbCallback; + aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + InternalProcessAllTriangles(&aabbCallback, aabbMin, aabbMax); aabbMin = aabbCallback.m_aabbMin; aabbMax = aabbCallback.m_aabbMax; } - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const +const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* serializer) const { - btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*) dataBuffer; + btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*)dataBuffer; trimeshData->m_numMeshParts = getNumSubParts(); @@ -226,29 +220,28 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s if (trimeshData->m_numMeshParts) { - btChunk* chunk = serializer->allocate(sizeof(btMeshPartData),trimeshData->m_numMeshParts); + btChunk* chunk = serializer->allocate(sizeof(btMeshPartData), trimeshData->m_numMeshParts); btMeshPartData* memPtr = (btMeshPartData*)chunk->m_oldPtr; - trimeshData->m_meshPartsPtr = (btMeshPartData *)serializer->getUniquePointer(memPtr); - + trimeshData->m_meshPartsPtr = (btMeshPartData*)serializer->getUniquePointer(memPtr); - // int numtotalphysicsverts = 0; - int part,graphicssubparts = getNumSubParts(); - const unsigned char * vertexbase; - const unsigned char * indexbase; + // int numtotalphysicsverts = 0; + int part, graphicssubparts = getNumSubParts(); + const unsigned char* vertexbase; + const unsigned char* indexbase; int indexstride; PHY_ScalarType type; PHY_ScalarType gfxindextype; - int stride,numverts,numtriangles; + int stride, numverts, numtriangles; int gfxindex; - // btVector3 triangle[3]; + // btVector3 triangle[3]; - // btVector3 meshScaling = getScaling(); + // btVector3 meshScaling = getScaling(); ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype - for (part=0;part<graphicssubparts ;part++,memPtr++) + for (part = 0; part < graphicssubparts; part++, memPtr++) { - getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part); - memPtr->m_numTriangles = numtriangles;//indices = 3*numtriangles + getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numtriangles, gfxindextype, part); + memPtr->m_numTriangles = numtriangles; //indices = 3*numtriangles memPtr->m_numVertices = numverts; memPtr->m_indices16 = 0; memPtr->m_indices32 = 0; @@ -257,44 +250,46 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s memPtr->m_vertices3f = 0; memPtr->m_vertices3d = 0; - switch (gfxindextype) { - case PHY_INTEGER: + case PHY_INTEGER: { - int numindices = numtriangles*3; - + int numindices = numtriangles * 3; + if (numindices) { - btChunk* chunk = serializer->allocate(sizeof(btIntIndexData),numindices); + btChunk* chunk = serializer->allocate(sizeof(btIntIndexData), numindices); btIntIndexData* tmpIndices = (btIntIndexData*)chunk->m_oldPtr; memPtr->m_indices32 = (btIntIndexData*)serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned int* tri_indices= (unsigned int*)(indexbase+gfxindex*indexstride); - tmpIndices[gfxindex*3].m_value = tri_indices[0]; - tmpIndices[gfxindex*3+1].m_value = tri_indices[1]; - tmpIndices[gfxindex*3+2].m_value = tri_indices[2]; + unsigned int* tri_indices = (unsigned int*)(indexbase + gfxindex * indexstride); + tmpIndices[gfxindex * 3].m_value = tri_indices[0]; + tmpIndices[gfxindex * 3 + 1].m_value = tri_indices[1]; + tmpIndices[gfxindex * 3 + 2].m_value = tri_indices[2]; } - serializer->finalizeChunk(chunk,"btIntIndexData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btIntIndexData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } - case PHY_SHORT: + case PHY_SHORT: { if (numtriangles) { - btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData),numtriangles); + btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData), numtriangles); btShortIntIndexTripletData* tmpIndices = (btShortIntIndexTripletData*)chunk->m_oldPtr; - memPtr->m_3indices16 = (btShortIntIndexTripletData*) serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + memPtr->m_3indices16 = (btShortIntIndexTripletData*)serializer->getUniquePointer(tmpIndices); + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned short int* tri_indices= (unsigned short int*)(indexbase+gfxindex*indexstride); + unsigned short int* tri_indices = (unsigned short int*)(indexbase + gfxindex * indexstride); tmpIndices[gfxindex].m_values[0] = tri_indices[0]; tmpIndices[gfxindex].m_values[1] = tri_indices[1]; tmpIndices[gfxindex].m_values[2] = tri_indices[2]; + // Fill padding with zeros to appease msan. + tmpIndices[gfxindex].m_pad[0] = 0; + tmpIndices[gfxindex].m_pad[1] = 0; } - serializer->finalizeChunk(chunk,"btShortIntIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btShortIntIndexTripletData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } @@ -302,21 +297,23 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s { if (numtriangles) { - btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData),numtriangles); + btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData), numtriangles); btCharIndexTripletData* tmpIndices = (btCharIndexTripletData*)chunk->m_oldPtr; - memPtr->m_3indices8 = (btCharIndexTripletData*) serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + memPtr->m_3indices8 = (btCharIndexTripletData*)serializer->getUniquePointer(tmpIndices); + for (gfxindex = 0; gfxindex < numtriangles; gfxindex++) { - unsigned char* tri_indices= (unsigned char*)(indexbase+gfxindex*indexstride); + unsigned char* tri_indices = (unsigned char*)(indexbase + gfxindex * indexstride); tmpIndices[gfxindex].m_values[0] = tri_indices[0]; tmpIndices[gfxindex].m_values[1] = tri_indices[1]; tmpIndices[gfxindex].m_values[2] = tri_indices[2]; + // Fill padding with zeros to appease msan. + tmpIndices[gfxindex].m_pad = 0; } - serializer->finalizeChunk(chunk,"btCharIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btCharIndexTripletData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } - default: + default: { btAssert(0); //unknown index type @@ -325,56 +322,58 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s switch (type) { - case PHY_FLOAT: - { - float* graphicsbase; - - if (numverts) - { - btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData),numverts); - btVector3FloatData* tmpVertices = (btVector3FloatData*) chunk->m_oldPtr; - memPtr->m_vertices3f = (btVector3FloatData *)serializer->getUniquePointer(tmpVertices); - for (int i=0;i<numverts;i++) - { - graphicsbase = (float*)(vertexbase+i*stride); - tmpVertices[i].m_floats[0] = graphicsbase[0]; - tmpVertices[i].m_floats[1] = graphicsbase[1]; - tmpVertices[i].m_floats[2] = graphicsbase[2]; - } - serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); - } - break; + case PHY_FLOAT: + { + float* graphicsbase; + + if (numverts) + { + btChunk* chunk = serializer->allocate(sizeof(btVector3FloatData), numverts); + btVector3FloatData* tmpVertices = (btVector3FloatData*)chunk->m_oldPtr; + memPtr->m_vertices3f = (btVector3FloatData*)serializer->getUniquePointer(tmpVertices); + for (int i = 0; i < numverts; i++) + { + graphicsbase = (float*)(vertexbase + i * stride); + tmpVertices[i].m_floats[0] = graphicsbase[0]; + tmpVertices[i].m_floats[1] = graphicsbase[1]; + tmpVertices[i].m_floats[2] = graphicsbase[2]; + } + serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); + } + break; } - case PHY_DOUBLE: + case PHY_DOUBLE: { if (numverts) { - btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData),numverts); - btVector3DoubleData* tmpVertices = (btVector3DoubleData*) chunk->m_oldPtr; - memPtr->m_vertices3d = (btVector3DoubleData *) serializer->getUniquePointer(tmpVertices); - for (int i=0;i<numverts;i++) - { - double* graphicsbase = (double*)(vertexbase+i*stride);//for now convert to float, might leave it at double - tmpVertices[i].m_floats[0] = graphicsbase[0]; - tmpVertices[i].m_floats[1] = graphicsbase[1]; - tmpVertices[i].m_floats[2] = graphicsbase[2]; - } - serializer->finalizeChunk(chunk,"btVector3DoubleData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); + btChunk* chunk = serializer->allocate(sizeof(btVector3DoubleData), numverts); + btVector3DoubleData* tmpVertices = (btVector3DoubleData*)chunk->m_oldPtr; + memPtr->m_vertices3d = (btVector3DoubleData*)serializer->getUniquePointer(tmpVertices); + for (int i = 0; i < numverts; i++) + { + double* graphicsbase = (double*)(vertexbase + i * stride); //for now convert to float, might leave it at double + tmpVertices[i].m_floats[0] = graphicsbase[0]; + tmpVertices[i].m_floats[1] = graphicsbase[1]; + tmpVertices[i].m_floats[2] = graphicsbase[2]; + } + serializer->finalizeChunk(chunk, "btVector3DoubleData", BT_ARRAY_CODE, (void*)chunk->m_oldPtr); } break; } - default: - btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); + default: + btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); } unLockReadOnlyVertexBase(part); } - serializer->finalizeChunk(chunk,"btMeshPartData",BT_ARRAY_CODE,chunk->m_oldPtr); + serializer->finalizeChunk(chunk, "btMeshPartData", BT_ARRAY_CODE, chunk->m_oldPtr); } + // Fill padding with zeros to appease msan. + memset(trimeshData->m_padding, 0, sizeof(trimeshData->m_padding)); m_scaling.serializeFloat(trimeshData->m_scaling); return "btStridingMeshInterfaceData"; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h index 9fbe139768d..68a41dfb450 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h @@ -20,110 +20,102 @@ subject to the following restrictions: #include "btTriangleCallback.h" #include "btConcaveShape.h" - - - - /// The btStridingMeshInterface is the interface class for high performance generic access to triangle meshes, used in combination with btBvhTriangleMeshShape and some other collision shapes. /// Using index striding of 3*sizeof(integer) it can use triangle arrays, using index striding of 1*sizeof(integer) it can handle triangle strips. /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. -ATTRIBUTE_ALIGNED16(class ) btStridingMeshInterface +ATTRIBUTE_ALIGNED16(class) +btStridingMeshInterface { - protected: - - btVector3 m_scaling; - - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btStridingMeshInterface() :m_scaling(btScalar(1.),btScalar(1.),btScalar(1.)) - { - - } - - virtual ~btStridingMeshInterface(); - - - - virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - ///brute force method to calculate aabb - void calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax); - - /// 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 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() const=0; - - virtual void preallocateVertices(int numverts)=0; - virtual void preallocateIndices(int numindices)=0; - - virtual bool hasPremadeAabb() const { return false; } - virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const - { - (void) aabbMin; - (void) aabbMax; - } - virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const - { - (void) aabbMin; - (void) aabbMax; - } - - const btVector3& getScaling() const { - return m_scaling; - } - void setScaling(const btVector3& scaling) - { - m_scaling = scaling; - } - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - +protected: + btVector3 m_scaling; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btStridingMeshInterface() : m_scaling(btScalar(1.), btScalar(1.), btScalar(1.)) + { + } + + virtual ~btStridingMeshInterface(); + + virtual void InternalProcessAllTriangles(btInternalTriangleIndexCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; + + ///brute force method to calculate aabb + void calculateAabbBruteForce(btVector3 & aabbMin, btVector3 & aabbMax); + + /// 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 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 separate subparts + /// each subpart has a continuous array of vertices and indices + virtual int getNumSubParts() const = 0; + + virtual void preallocateVertices(int numverts) = 0; + virtual void preallocateIndices(int numindices) = 0; + + virtual bool hasPremadeAabb() const { return false; } + virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax) const + { + (void)aabbMin; + (void)aabbMax; + } + virtual void getPremadeAabb(btVector3 * aabbMin, btVector3 * aabbMax) const + { + (void)aabbMin; + (void)aabbMax; + } + + const btVector3& getScaling() const + { + return m_scaling; + } + void setScaling(const btVector3& scaling) + { + m_scaling = scaling; + } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; }; -struct btIntIndexData +struct btIntIndexData { - int m_value; + int m_value; }; -struct btShortIntIndexData +struct btShortIntIndexData { short m_value; char m_pad[2]; }; -struct btShortIntIndexTripletData +struct btShortIntIndexTripletData { - short m_values[3]; - char m_pad[2]; + short m_values[3]; + char m_pad[2]; }; -struct btCharIndexTripletData +struct btCharIndexTripletData { unsigned char m_values[3]; - char m_pad; + char m_pad; }; +// clang-format off ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btMeshPartData @@ -151,14 +143,11 @@ struct btStridingMeshInterfaceData char m_padding[4]; }; +// clang-format on - - -SIMD_FORCE_INLINE int btStridingMeshInterface::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btStridingMeshInterface::calculateSerializeBufferSize() const { return sizeof(btStridingMeshInterfaceData); } - - -#endif //BT_STRIDING_MESHINTERFACE_H +#endif //BT_STRIDING_MESHINTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp index 52f346bf726..c4d33c429f0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp @@ -16,29 +16,29 @@ subject to the following restrictions: #include "btTetrahedronShape.h" #include "LinearMath/btMatrix3x3.h" -btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); addVertex(pt1); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); @@ -46,8 +46,8 @@ m_numVertices(0) addVertex(pt2); } -btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3) : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) +btBU_Simplex1to4::btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2, const btVector3& pt3) : btPolyhedralConvexAabbCachingShape(), + m_numVertices(0) { m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; addVertex(pt0); @@ -56,17 +56,16 @@ m_numVertices(0) addVertex(pt3); } - -void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btBU_Simplex1to4::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { #if 1 - btPolyhedralConvexAabbCachingShape::getAabb(t,aabbMin,aabbMax); + btPolyhedralConvexAabbCachingShape::getAabb(t, aabbMin, aabbMax); #else - aabbMin.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); - aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); + aabbMin.setValue(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); + aabbMax.setValue(-BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT); //just transform the vertices in worldspace, and take their AABB - for (int i=0;i<m_numVertices;i++) + for (int i = 0; i < m_numVertices; i++) { btVector3 worldVertex = t(m_vertices[i]); aabbMin.setMin(worldVertex); @@ -75,18 +74,13 @@ void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3 #endif } - - - - void btBU_Simplex1to4::addVertex(const btVector3& pt) { m_vertices[m_numVertices++] = pt; recalcLocalAabb(); } - -int btBU_Simplex1to4::getNumVertices() const +int btBU_Simplex1to4::getNumVertices() const { return m_numVertices; } @@ -97,122 +91,113 @@ int btBU_Simplex1to4::getNumEdges() const switch (m_numVertices) { - case 0: - return 0; - case 1: return 0; - case 2: return 1; - case 3: return 3; - case 4: return 6; - - + case 0: + return 0; + case 1: + return 0; + case 2: + return 1; + case 3: + return 3; + case 4: + return 6; } return 0; } -void btBU_Simplex1to4::getEdge(int i,btVector3& pa,btVector3& pb) const +void btBU_Simplex1to4::getEdge(int i, btVector3& pa, btVector3& pb) const { - - switch (m_numVertices) + switch (m_numVertices) { - - case 2: - pa = m_vertices[0]; - pb = m_vertices[1]; - break; - case 3: - switch (i) - { - case 0: - pa = m_vertices[0]; - pb = m_vertices[1]; - break; - case 1: - pa = m_vertices[1]; - pb = m_vertices[2]; - break; case 2: - pa = m_vertices[2]; - pb = m_vertices[0]; - break; - - } - break; - case 4: - switch (i) - { - case 0: pa = m_vertices[0]; pb = m_vertices[1]; break; - case 1: - pa = m_vertices[1]; - pb = m_vertices[2]; - break; - case 2: - pa = m_vertices[2]; - pb = m_vertices[0]; - break; case 3: - pa = m_vertices[0]; - pb = m_vertices[3]; + switch (i) + { + case 0: + pa = m_vertices[0]; + pb = m_vertices[1]; + break; + case 1: + pa = m_vertices[1]; + pb = m_vertices[2]; + break; + case 2: + pa = m_vertices[2]; + pb = m_vertices[0]; + break; + } break; case 4: - pa = m_vertices[1]; - pb = m_vertices[3]; - break; - case 5: - pa = m_vertices[2]; - pb = m_vertices[3]; - break; - } - + switch (i) + { + case 0: + pa = m_vertices[0]; + pb = m_vertices[1]; + break; + case 1: + pa = m_vertices[1]; + pb = m_vertices[2]; + break; + case 2: + pa = m_vertices[2]; + pb = m_vertices[0]; + break; + case 3: + pa = m_vertices[0]; + pb = m_vertices[3]; + break; + case 4: + pa = m_vertices[1]; + pb = m_vertices[3]; + break; + case 5: + pa = m_vertices[2]; + pb = m_vertices[3]; + break; + } } - - - - } -void btBU_Simplex1to4::getVertex(int i,btVector3& vtx) const +void btBU_Simplex1to4::getVertex(int i, btVector3& vtx) const { vtx = m_vertices[i]; } -int btBU_Simplex1to4::getNumPlanes() const +int btBU_Simplex1to4::getNumPlanes() const { switch (m_numVertices) { - case 0: + case 0: return 0; - case 1: + case 1: return 0; - case 2: + case 2: return 0; - case 3: + case 3: return 2; - case 4: + case 4: return 4; - default: + default: { } } return 0; } - -void btBU_Simplex1to4::getPlane(btVector3&, btVector3& ,int ) const +void btBU_Simplex1to4::getPlane(btVector3&, btVector3&, int) const { - } -int btBU_Simplex1to4::getIndex(int ) const +int btBU_Simplex1to4::getIndex(int) const { return 0; } -bool btBU_Simplex1to4::isInside(const btVector3& ,btScalar ) const +bool btBU_Simplex1to4::isInside(const btVector3&, btScalar) const { return false; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h index b69209835ef..f5e2209ed09 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h @@ -16,61 +16,56 @@ subject to the following restrictions: #ifndef BT_SIMPLEX_1TO4_SHAPE #define BT_SIMPLEX_1TO4_SHAPE - #include "btPolyhedralConvexShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" - ///The btBU_Simplex1to4 implements tetrahedron, triangle, line, vertex collision shapes. In most cases it is better to use btConvexHullShape instead. -ATTRIBUTE_ALIGNED16(class) btBU_Simplex1to4 : public btPolyhedralConvexAabbCachingShape +ATTRIBUTE_ALIGNED16(class) +btBU_Simplex1to4 : public btPolyhedralConvexAabbCachingShape { protected: - - int m_numVertices; - btVector3 m_vertices[4]; + int m_numVertices; + btVector3 m_vertices[4]; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btBU_Simplex1to4(); btBU_Simplex1to4(const btVector3& pt0); - btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1); - btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2); - btBU_Simplex1to4(const btVector3& pt0,const btVector3& pt1,const btVector3& pt2,const btVector3& pt3); + btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1); + btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2); + btBU_Simplex1to4(const btVector3& pt0, const btVector3& pt1, const btVector3& pt2, const btVector3& pt3); - - void reset() + void reset() { m_numVertices = 0; } - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; void addVertex(const btVector3& pt); //PolyhedralConvexShape interface - virtual int getNumVertices() const; + virtual int getNumVertices() const; virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - - virtual void getVertex(int i,btVector3& vtx) const; + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const; - virtual int getNumPlanes() const; + virtual void getVertex(int i, btVector3& vtx) const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const; + virtual int getNumPlanes() const; - virtual int getIndex(int i) const; + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + virtual int getIndex(int i) const; + virtual bool isInside(const btVector3& pt, btScalar tolerance) const; ///getName is for debugging - virtual const char* getName()const { return "btBU_Simplex1to4";} - + virtual const char* getName() const { return "btBU_Simplex1to4"; } }; -#endif //BT_SIMPLEX_1TO4_SHAPE +#endif //BT_SIMPLEX_1TO4_SHAPE diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp index 3027e65b256..3b6db2b39fa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp @@ -15,21 +15,14 @@ subject to the following restrictions: #include "btTriangleBuffer.h" - - - - - - -void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex) +void btTriangleBuffer::processTriangle(btVector3* triangle, int partId, int triangleIndex) { - btTriangle tri; - tri.m_vertex0 = triangle[0]; - tri.m_vertex1 = triangle[1]; - tri.m_vertex2 = triangle[2]; - tri.m_partId = partId; - tri.m_triangleIndex = triangleIndex; - - m_triangleBuffer.push_back(tri); + btTriangle tri; + tri.m_vertex0 = triangle[0]; + tri.m_vertex1 = triangle[1]; + tri.m_vertex2 = triangle[2]; + tri.m_partId = partId; + tri.m_triangleIndex = triangleIndex; + + m_triangleBuffer.push_back(tri); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index b71fc8b3746..a89b9cd8a48 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -19,13 +19,13 @@ subject to the following restrictions: #include "btTriangleCallback.h" #include "LinearMath/btAlignedObjectArray.h" -struct btTriangle +struct btTriangle { - btVector3 m_vertex0; - btVector3 m_vertex1; - btVector3 m_vertex2; - int m_partId; - int m_triangleIndex; + btVector3 m_vertex0; + btVector3 m_vertex1; + btVector3 m_vertex2; + int m_partId; + int m_triangleIndex; }; ///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles' @@ -39,31 +39,25 @@ struct btTriangle /// } class btTriangleBuffer : public btTriangleCallback { + btAlignedObjectArray<btTriangle> m_triangleBuffer; - btAlignedObjectArray<btTriangle> m_triangleBuffer; - public: - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - - int getNumTriangles() const + + int getNumTriangles() const { return int(m_triangleBuffer.size()); } - - const btTriangle& getTriangle(int index) const + + const btTriangle& getTriangle(int index) const { return m_triangleBuffer[index]; } - void clearBuffer() + void clearBuffer() { m_triangleBuffer.clear(); } - }; - -#endif //BT_TRIANGLE_BUFFER_H - +#endif //BT_TRIANGLE_BUFFER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp index f558bf6d241..5bd2c595fed 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp @@ -17,12 +17,8 @@ subject to the following restrictions: btTriangleCallback::~btTriangleCallback() { - } - btInternalTriangleIndexCallback::~btInternalTriangleIndexCallback() { - } - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h index 461c57f8773..d3644891ee6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h @@ -18,13 +18,11 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" - ///The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTriangles. ///This callback is called by processAllTriangles for all btConcaveShape derived class, such as btBvhTriangleMeshShape, btStaticPlaneShape and btHeightfieldTerrainShape. class btTriangleCallback { public: - virtual ~btTriangleCallback(); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) = 0; }; @@ -32,11 +30,8 @@ public: class btInternalTriangleIndexCallback { public: - virtual ~btInternalTriangleIndexCallback(); - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) = 0; + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) = 0; }; - - -#endif //BT_TRIANGLE_CALLBACK_H +#endif //BT_TRIANGLE_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index a665024cb61..dae42551941 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -15,81 +15,76 @@ subject to the following restrictions: #include "btTriangleIndexVertexArray.h" -btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride) -: m_hasAabb(0) +btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, int numVertices, btScalar* vertexBase, int vertexStride) + : m_hasAabb(0) { btIndexedMesh mesh; mesh.m_numTriangles = numTriangles; - mesh.m_triangleIndexBase = (const unsigned char *)triangleIndexBase; + mesh.m_triangleIndexBase = (const unsigned char*)triangleIndexBase; mesh.m_triangleIndexStride = triangleIndexStride; mesh.m_numVertices = numVertices; - mesh.m_vertexBase = (const unsigned char *)vertexBase; + mesh.m_vertexBase = (const unsigned char*)vertexBase; mesh.m_vertexStride = vertexStride; addIndexedMesh(mesh); - } btTriangleIndexVertexArray::~btTriangleIndexVertexArray() { - } -void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) +void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char** vertexbase, int& numverts, PHY_ScalarType& type, int& vertexStride, unsigned char** indexbase, int& indexstride, int& numfaces, PHY_ScalarType& indicestype, int subpart) { - btAssert(subpart< getNumSubParts() ); + btAssert(subpart < getNumSubParts()); btIndexedMesh& mesh = m_indexedMeshes[subpart]; numverts = mesh.m_numVertices; - (*vertexbase) = (unsigned char *) mesh.m_vertexBase; + (*vertexbase) = (unsigned char*)mesh.m_vertexBase; - type = mesh.m_vertexType; + type = mesh.m_vertexType; vertexStride = mesh.m_vertexStride; numfaces = mesh.m_numTriangles; - (*indexbase) = (unsigned char *)mesh.m_triangleIndexBase; + (*indexbase) = (unsigned char*)mesh.m_triangleIndexBase; indexstride = mesh.m_triangleIndexStride; indicestype = mesh.m_indexType; } -void btTriangleIndexVertexArray::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 +void btTriangleIndexVertexArray::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 { const btIndexedMesh& mesh = m_indexedMeshes[subpart]; numverts = mesh.m_numVertices; - (*vertexbase) = (const unsigned char *)mesh.m_vertexBase; + (*vertexbase) = (const unsigned char*)mesh.m_vertexBase; + + type = mesh.m_vertexType; - type = mesh.m_vertexType; - vertexStride = mesh.m_vertexStride; numfaces = mesh.m_numTriangles; - (*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase; + (*indexbase) = (const unsigned char*)mesh.m_triangleIndexBase; indexstride = mesh.m_triangleIndexStride; indicestype = mesh.m_indexType; } -bool btTriangleIndexVertexArray::hasPremadeAabb() const +bool btTriangleIndexVertexArray::hasPremadeAabb() const { return (m_hasAabb == 1); } - -void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const +void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax) const { m_aabbMin = aabbMin; m_aabbMax = aabbMax; - m_hasAabb = 1; // this is intentionally an int see notes in header + m_hasAabb = 1; // this is intentionally an int see notes in header } -void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const +void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax) const { *aabbMin = m_aabbMin; *aabbMax = m_aabbMax; } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index 9e1544e87a4..556aa3fef4d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -20,62 +20,59 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btScalar.h" - ///The btIndexedMesh indexes a single vertex and index array. Multiple btIndexedMesh objects can be passed into a btTriangleIndexVertexArray using addIndexedMesh. ///Instead of the number of indices, we pass the number of triangles. -ATTRIBUTE_ALIGNED16( struct) btIndexedMesh +ATTRIBUTE_ALIGNED16(struct) +btIndexedMesh { BT_DECLARE_ALIGNED_ALLOCATOR(); - int m_numTriangles; - const unsigned char * m_triangleIndexBase; - // Size in byte of the indices for one triangle (3*sizeof(index_type) if the indices are tightly packed) - int m_triangleIndexStride; - int m_numVertices; - const unsigned char * m_vertexBase; - // Size of a vertex, in bytes - int m_vertexStride; - - // The index type is set when adding an indexed mesh to the - // btTriangleIndexVertexArray, do not set it manually - PHY_ScalarType m_indexType; - - // The vertex type has a default type similar to Bullet's precision mode (float or double) - // but can be set manually if you for example run Bullet with double precision but have - // mesh data in single precision.. - PHY_ScalarType m_vertexType; - - - btIndexedMesh() - :m_indexType(PHY_INTEGER), + int m_numTriangles; + const unsigned char* m_triangleIndexBase; + // Size in byte of the indices for one triangle (3*sizeof(index_type) if the indices are tightly packed) + int m_triangleIndexStride; + int m_numVertices; + const unsigned char* m_vertexBase; + // Size of a vertex, in bytes + int m_vertexStride; + + // The index type is set when adding an indexed mesh to the + // btTriangleIndexVertexArray, do not set it manually + PHY_ScalarType m_indexType; + + // The vertex type has a default type similar to Bullet's precision mode (float or double) + // but can be set manually if you for example run Bullet with double precision but have + // mesh data in single precision.. + PHY_ScalarType m_vertexType; + + btIndexedMesh() + : m_indexType(PHY_INTEGER), #ifdef BT_USE_DOUBLE_PRECISION - m_vertexType(PHY_DOUBLE) -#else // BT_USE_DOUBLE_PRECISION - m_vertexType(PHY_FLOAT) -#endif // BT_USE_DOUBLE_PRECISION - { - } -} -; - + m_vertexType(PHY_DOUBLE) +#else // BT_USE_DOUBLE_PRECISION + m_vertexType(PHY_FLOAT) +#endif // BT_USE_DOUBLE_PRECISION + { + } +}; -typedef btAlignedObjectArray<btIndexedMesh> IndexedMeshArray; +typedef btAlignedObjectArray<btIndexedMesh> IndexedMeshArray; ///The btTriangleIndexVertexArray allows to access multiple triangle meshes, by indexing into existing triangle/index arrays. ///Additional meshes can be added using addIndexedMesh -///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays. +///No duplicate is made of the vertex/index data, it only indexes into external vertex/index arrays. ///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray. -ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface +ATTRIBUTE_ALIGNED16(class) +btTriangleIndexVertexArray : public btStridingMeshInterface { protected: - IndexedMeshArray m_indexedMeshes; + IndexedMeshArray m_indexedMeshes; int m_pad[2]; - mutable int m_hasAabb; // using int instead of bool to maintain alignment + mutable int m_hasAabb; // using int instead of bool to maintain alignment mutable btVector3 m_aabbMin; mutable btVector3 m_aabbMax; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); btTriangleIndexVertexArray() : m_hasAabb(0) @@ -85,49 +82,47 @@ public: virtual ~btTriangleIndexVertexArray(); //just to be backwards compatible - btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,btScalar* vertexBase,int vertexStride); - - void addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER) + btTriangleIndexVertexArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, int numVertices, btScalar* vertexBase, int vertexStride); + + void addIndexedMesh(const btIndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER) { m_indexedMeshes.push_back(mesh); - m_indexedMeshes[m_indexedMeshes.size()-1].m_indexType = indexType; + m_indexedMeshes[m_indexedMeshes.size() - 1].m_indexType = indexType; } - - - 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; + 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) {(void)subpart;} + virtual void unLockVertexBase(int subpart) { (void)subpart; } - virtual void unLockReadOnlyVertexBase(int subpart) const {(void)subpart;} + virtual void unLockReadOnlyVertexBase(int subpart) const { (void)subpart; } - /// getNumSubParts returns the number of seperate subparts + /// getNumSubParts returns the number of separate subparts /// each subpart has a continuous array of vertices and indices - virtual int getNumSubParts() const { + virtual int getNumSubParts() const + { return (int)m_indexedMeshes.size(); } - IndexedMeshArray& getIndexedMeshArray() + IndexedMeshArray& getIndexedMeshArray() { return m_indexedMeshes; } - const IndexedMeshArray& getIndexedMeshArray() const + const IndexedMeshArray& getIndexedMeshArray() const { return m_indexedMeshes; } - virtual void preallocateVertices(int numverts){(void) numverts;} - virtual void preallocateIndices(int numindices){(void) numindices;} - - virtual bool hasPremadeAabb() const; - virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const; - virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const; + virtual void preallocateVertices(int numverts) { (void)numverts; } + virtual void preallocateIndices(int numindices) { (void)numindices; } -} -; + virtual bool hasPremadeAabb() const; + virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax) const; + virtual void getPremadeAabb(btVector3 * aabbMin, btVector3 * aabbMax) const; +}; -#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H +#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp index dc562941ad6..4bf133d7ac7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp @@ -17,70 +17,68 @@ subject to the following restrictions: #include "btTriangleIndexVertexMaterialArray.h" -btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, - int numVertices,btScalar* vertexBase,int vertexStride, - int numMaterials, unsigned char* materialBase, int materialStride, - int* triangleMaterialsBase, int materialIndexStride) : -btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride) +btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, + int numVertices, btScalar* vertexBase, int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride) : btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride) { - btMaterialProperties mat; + btMaterialProperties mat; - mat.m_numMaterials = numMaterials; - mat.m_materialBase = materialBase; - mat.m_materialStride = materialStride; + mat.m_numMaterials = numMaterials; + mat.m_materialBase = materialBase; + mat.m_materialStride = materialStride; #ifdef BT_USE_DOUBLE_PRECISION - mat.m_materialType = PHY_DOUBLE; + mat.m_materialType = PHY_DOUBLE; #else - mat.m_materialType = PHY_FLOAT; + mat.m_materialType = PHY_FLOAT; #endif - mat.m_numTriangles = numTriangles; - mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase; - mat.m_triangleMaterialStride = materialIndexStride; - mat.m_triangleType = PHY_INTEGER; + mat.m_numTriangles = numTriangles; + mat.m_triangleMaterialsBase = (unsigned char*)triangleMaterialsBase; + mat.m_triangleMaterialStride = materialIndexStride; + mat.m_triangleType = PHY_INTEGER; - addMaterialProperties(mat); + addMaterialProperties(mat); } - -void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) +void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) { - btAssert(subpart< getNumSubParts() ); + btAssert(subpart < getNumSubParts()); - btMaterialProperties& mats = m_materials[subpart]; + btMaterialProperties& mats = m_materials[subpart]; - numMaterials = mats.m_numMaterials; - (*materialBase) = (unsigned char *) mats.m_materialBase; + numMaterials = mats.m_numMaterials; + (*materialBase) = (unsigned char*)mats.m_materialBase; #ifdef BT_USE_DOUBLE_PRECISION - materialType = PHY_DOUBLE; + materialType = PHY_DOUBLE; #else - materialType = PHY_FLOAT; + materialType = PHY_FLOAT; #endif - materialStride = mats.m_materialStride; + materialStride = mats.m_materialStride; - numTriangles = mats.m_numTriangles; - (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase; - triangleMaterialStride = mats.m_triangleMaterialStride; - triangleType = mats.m_triangleType; + numTriangles = mats.m_numTriangles; + (*triangleMaterialBase) = (unsigned char*)mats.m_triangleMaterialsBase; + triangleMaterialStride = mats.m_triangleMaterialStride; + triangleType = mats.m_triangleType; } -void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) +void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + const unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) { - btMaterialProperties& mats = m_materials[subpart]; + btMaterialProperties& mats = m_materials[subpart]; - numMaterials = mats.m_numMaterials; - (*materialBase) = (const unsigned char *) mats.m_materialBase; + numMaterials = mats.m_numMaterials; + (*materialBase) = (const unsigned char*)mats.m_materialBase; #ifdef BT_USE_DOUBLE_PRECISION - materialType = PHY_DOUBLE; + materialType = PHY_DOUBLE; #else - materialType = PHY_FLOAT; + materialType = PHY_FLOAT; #endif - materialStride = mats.m_materialStride; + materialStride = mats.m_materialStride; - numTriangles = mats.m_numTriangles; - (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase; - triangleMaterialStride = mats.m_triangleMaterialStride; - triangleType = mats.m_triangleType; + numTriangles = mats.m_numTriangles; + (*triangleMaterialBase) = (const unsigned char*)mats.m_triangleMaterialsBase; + triangleMaterialStride = mats.m_triangleMaterialStride; + triangleType = mats.m_triangleType; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h index ba4f7b46076..315b1e21f35 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h @@ -20,26 +20,26 @@ subject to the following restrictions: #include "btTriangleIndexVertexArray.h" - -ATTRIBUTE_ALIGNED16( struct) btMaterialProperties +ATTRIBUTE_ALIGNED16(struct) +btMaterialProperties { - ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution - int m_numMaterials; - const unsigned char * m_materialBase; - int m_materialStride; - PHY_ScalarType m_materialType; - ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're - /// padding the structure, it can be reproduced at no real cost - ///m_triangleMaterials =====> 1 integer value makes up one entry - /// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5 - int m_numTriangles; - const unsigned char * m_triangleMaterialsBase; - int m_triangleMaterialStride; - ///m_triangleType <========== Automatically set in addMaterialProperties - PHY_ScalarType m_triangleType; + ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution + int m_numMaterials; + const unsigned char* m_materialBase; + int m_materialStride; + PHY_ScalarType m_materialType; + ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're + /// padding the structure, it can be reproduced at no real cost + ///m_triangleMaterials =====> 1 integer value makes up one entry + /// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5 + int m_numTriangles; + const unsigned char* m_triangleMaterialsBase; + int m_triangleMaterialStride; + ///m_triangleType <========== Automatically set in addMaterialProperties + PHY_ScalarType m_triangleType; }; -typedef btAlignedObjectArray<btMaterialProperties> MaterialArray; +typedef btAlignedObjectArray<btMaterialProperties> MaterialArray; ///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray ///The addition of a material array allows for the utilization of the partID and @@ -47,38 +47,37 @@ typedef btAlignedObjectArray<btMaterialProperties> MaterialArray; ///TriangleIndexVertexArray, no duplicate is made of the material data, so it ///is the users responsibility to maintain the array during the lifetime of the ///TriangleIndexVertexMaterialArray. -ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray +ATTRIBUTE_ALIGNED16(class) +btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray { protected: - MaterialArray m_materials; - + MaterialArray m_materials; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btTriangleIndexVertexMaterialArray() + btTriangleIndexVertexMaterialArray() { } - btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, - int numVertices,btScalar* vertexBase,int vertexStride, - int numMaterials, unsigned char* materialBase, int materialStride, - int* triangleMaterialsBase, int materialIndexStride); - - virtual ~btTriangleIndexVertexMaterialArray() {} + btTriangleIndexVertexMaterialArray(int numTriangles, int* triangleIndexBase, int triangleIndexStride, + int numVertices, btScalar* vertexBase, int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride); - void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER) - { - m_materials.push_back(mat); - m_materials[m_materials.size()-1].m_triangleType = triangleType; - } + virtual ~btTriangleIndexVertexMaterialArray() {} - virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType ,int subpart = 0); + void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER) + { + m_materials.push_back(mat); + m_materials[m_materials.size() - 1].m_triangleType = triangleType; + } - virtual void getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0); + virtual void getLockedMaterialBase(unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0); -} -; + virtual void getLockedReadOnlyMaterialBase(const unsigned char** materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + const unsigned char** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0); +}; -#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H +#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h index 17deef89d37..8ee35ef5faf 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h @@ -16,11 +16,9 @@ subject to the following restrictions: #ifndef _BT_TRIANGLE_INFO_MAP_H #define _BT_TRIANGLE_INFO_MAP_H - #include "LinearMath/btHashMap.h" #include "LinearMath/btSerializer.h" - ///for btTriangleInfo m_flags #define TRI_INFO_V0V1_CONVEX 1 #define TRI_INFO_V1V2_CONVEX 2 @@ -30,61 +28,58 @@ subject to the following restrictions: #define TRI_INFO_V1V2_SWAP_NORMALB 16 #define TRI_INFO_V2V0_SWAP_NORMALB 32 - ///The btTriangleInfo structure stores information to adjust collision normals to avoid collisions against internal edges -///it can be generated using -struct btTriangleInfo +///it can be generated using +struct btTriangleInfo { btTriangleInfo() { m_edgeV0V1Angle = SIMD_2_PI; m_edgeV1V2Angle = SIMD_2_PI; m_edgeV2V0Angle = SIMD_2_PI; - m_flags=0; + m_flags = 0; } - int m_flags; - - btScalar m_edgeV0V1Angle; - btScalar m_edgeV1V2Angle; - btScalar m_edgeV2V0Angle; + int m_flags; + btScalar m_edgeV0V1Angle; + btScalar m_edgeV1V2Angle; + btScalar m_edgeV2V0Angle; }; -typedef btHashMap<btHashInt,btTriangleInfo> btInternalTriangleInfoMap; - +typedef btHashMap<btHashInt, btTriangleInfo> btInternalTriangleInfoMap; ///The btTriangleInfoMap stores edge angle information for some triangles. You can compute this information yourself or using btGenerateInternalEdgeInfo. -struct btTriangleInfoMap : public btInternalTriangleInfoMap +struct btTriangleInfoMap : public btInternalTriangleInfoMap { - btScalar m_convexEpsilon;///used to determine if an edge or contact normal is convex, using the dot product - btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle - btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared' - btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge" - btScalar m_maxEdgeAngleThreshold; //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold - btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold) - - + btScalar m_convexEpsilon; ///used to determine if an edge or contact normal is convex, using the dot product + btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle + btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared' + btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge" + btScalar m_maxEdgeAngleThreshold; //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold + btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold) + btTriangleInfoMap() { m_convexEpsilon = 0.00f; m_planarEpsilon = 0.0001f; - m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001); + m_equalVertexThreshold = btScalar(0.0001) * btScalar(0.0001); m_edgeDistanceThreshold = btScalar(0.1); - m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001); + m_zeroAreaThreshold = btScalar(0.0001) * btScalar(0.0001); m_maxEdgeAngleThreshold = SIMD_2_PI; } virtual ~btTriangleInfoMap() {} - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - void deSerialize(struct btTriangleInfoMapData& data); + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + void deSerialize(struct btTriangleInfoMapData& data); }; +// clang-format off + ///those fields have to be float and not btScalar for the serialization to work properly struct btTriangleInfoData { @@ -114,128 +109,130 @@ struct btTriangleInfoMapData char m_padding[4]; }; -SIMD_FORCE_INLINE int btTriangleInfoMap::calculateSerializeBufferSize() const +// clang-format on + +SIMD_FORCE_INLINE int btTriangleInfoMap::calculateSerializeBufferSize() const { return sizeof(btTriangleInfoMapData); } ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btTriangleInfoMap::serialize(void* dataBuffer, btSerializer* serializer) const +SIMD_FORCE_INLINE const char* btTriangleInfoMap::serialize(void* dataBuffer, btSerializer* serializer) const { - btTriangleInfoMapData* tmapData = (btTriangleInfoMapData*) dataBuffer; + btTriangleInfoMapData* tmapData = (btTriangleInfoMapData*)dataBuffer; tmapData->m_convexEpsilon = (float)m_convexEpsilon; tmapData->m_planarEpsilon = (float)m_planarEpsilon; - tmapData->m_equalVertexThreshold =(float) m_equalVertexThreshold; + tmapData->m_equalVertexThreshold = (float)m_equalVertexThreshold; tmapData->m_edgeDistanceThreshold = (float)m_edgeDistanceThreshold; tmapData->m_zeroAreaThreshold = (float)m_zeroAreaThreshold; - + tmapData->m_hashTableSize = m_hashTable.size(); tmapData->m_hashTablePtr = tmapData->m_hashTableSize ? (int*)serializer->getUniquePointer((void*)&m_hashTable[0]) : 0; if (tmapData->m_hashTablePtr) - { + { //serialize an int buffer int sz = sizeof(int); int numElem = tmapData->m_hashTableSize; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { *memPtr = m_hashTable[i]; } - serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_hashTable[0]); - + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_hashTable[0]); } tmapData->m_nextSize = m_next.size(); - tmapData->m_nextPtr = tmapData->m_nextSize? (int*)serializer->getUniquePointer((void*)&m_next[0]): 0; + tmapData->m_nextPtr = tmapData->m_nextSize ? (int*)serializer->getUniquePointer((void*)&m_next[0]) : 0; if (tmapData->m_nextPtr) { int sz = sizeof(int); int numElem = tmapData->m_nextSize; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { *memPtr = m_next[i]; } - serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_next[0]); + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_next[0]); } - + tmapData->m_numValues = m_valueArray.size(); - tmapData->m_valueArrayPtr = tmapData->m_numValues ? (btTriangleInfoData*)serializer->getUniquePointer((void*)&m_valueArray[0]): 0; + tmapData->m_valueArrayPtr = tmapData->m_numValues ? (btTriangleInfoData*)serializer->getUniquePointer((void*)&m_valueArray[0]) : 0; if (tmapData->m_valueArrayPtr) { int sz = sizeof(btTriangleInfoData); int numElem = tmapData->m_numValues; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); btTriangleInfoData* memPtr = (btTriangleInfoData*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { memPtr->m_edgeV0V1Angle = (float)m_valueArray[i].m_edgeV0V1Angle; memPtr->m_edgeV1V2Angle = (float)m_valueArray[i].m_edgeV1V2Angle; memPtr->m_edgeV2V0Angle = (float)m_valueArray[i].m_edgeV2V0Angle; memPtr->m_flags = m_valueArray[i].m_flags; } - serializer->finalizeChunk(chunk,"btTriangleInfoData",BT_ARRAY_CODE,(void*) &m_valueArray[0]); + serializer->finalizeChunk(chunk, "btTriangleInfoData", BT_ARRAY_CODE, (void*)&m_valueArray[0]); } - + tmapData->m_numKeys = m_keyArray.size(); tmapData->m_keyArrayPtr = tmapData->m_numKeys ? (int*)serializer->getUniquePointer((void*)&m_keyArray[0]) : 0; if (tmapData->m_keyArrayPtr) { int sz = sizeof(int); int numElem = tmapData->m_numValues; - btChunk* chunk = serializer->allocate(sz,numElem); + btChunk* chunk = serializer->allocate(sz, numElem); int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;i<numElem;i++,memPtr++) + for (int i = 0; i < numElem; i++, memPtr++) { *memPtr = m_keyArray[i].getUid1(); } - serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*) &m_keyArray[0]); - + serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_keyArray[0]); } - return "btTriangleInfoMapData"; -} + // Fill padding with zeros to appease msan. + tmapData->m_padding[0] = 0; + tmapData->m_padding[1] = 0; + tmapData->m_padding[2] = 0; + tmapData->m_padding[3] = 0; + return "btTriangleInfoMapData"; +} ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE void btTriangleInfoMap::deSerialize(btTriangleInfoMapData& tmapData ) +SIMD_FORCE_INLINE void btTriangleInfoMap::deSerialize(btTriangleInfoMapData& tmapData) { - - m_convexEpsilon = tmapData.m_convexEpsilon; m_planarEpsilon = tmapData.m_planarEpsilon; m_equalVertexThreshold = tmapData.m_equalVertexThreshold; m_edgeDistanceThreshold = tmapData.m_edgeDistanceThreshold; m_zeroAreaThreshold = tmapData.m_zeroAreaThreshold; m_hashTable.resize(tmapData.m_hashTableSize); - int i =0; - for (i=0;i<tmapData.m_hashTableSize;i++) + int i = 0; + for (i = 0; i < tmapData.m_hashTableSize; i++) { m_hashTable[i] = tmapData.m_hashTablePtr[i]; } m_next.resize(tmapData.m_nextSize); - for (i=0;i<tmapData.m_nextSize;i++) + for (i = 0; i < tmapData.m_nextSize; i++) { m_next[i] = tmapData.m_nextPtr[i]; } m_valueArray.resize(tmapData.m_numValues); - for (i=0;i<tmapData.m_numValues;i++) + for (i = 0; i < tmapData.m_numValues; i++) { m_valueArray[i].m_edgeV0V1Angle = tmapData.m_valueArrayPtr[i].m_edgeV0V1Angle; m_valueArray[i].m_edgeV1V2Angle = tmapData.m_valueArrayPtr[i].m_edgeV1V2Angle; m_valueArray[i].m_edgeV2V0Angle = tmapData.m_valueArrayPtr[i].m_edgeV2V0Angle; m_valueArray[i].m_flags = tmapData.m_valueArrayPtr[i].m_flags; } - - m_keyArray.resize(tmapData.m_numKeys,btHashInt(0)); - for (i=0;i<tmapData.m_numKeys;i++) + + m_keyArray.resize(tmapData.m_numKeys, btHashInt(0)); + for (i = 0; i < tmapData.m_numKeys; i++) { m_keyArray[i].setUid1(tmapData.m_keyArrayPtr[i]); } } - -#endif //_BT_TRIANGLE_INFO_MAP_H +#endif //_BT_TRIANGLE_INFO_MAP_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp index e4de7320930..abd8c227866 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -13,38 +13,36 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btTriangleMesh.h" - - -btTriangleMesh::btTriangleMesh (bool use32bitIndices,bool use4componentVertices) -:m_use32bitIndices(use32bitIndices), -m_use4componentVertices(use4componentVertices), -m_weldingThreshold(0.0) +btTriangleMesh::btTriangleMesh(bool use32bitIndices, bool use4componentVertices) + : m_use32bitIndices(use32bitIndices), + m_use4componentVertices(use4componentVertices), + m_weldingThreshold(0.0) { btIndexedMesh meshIndex; meshIndex.m_numTriangles = 0; meshIndex.m_numVertices = 0; meshIndex.m_indexType = PHY_INTEGER; meshIndex.m_triangleIndexBase = 0; - meshIndex.m_triangleIndexStride = 3*sizeof(int); + meshIndex.m_triangleIndexStride = 3 * sizeof(int); meshIndex.m_vertexBase = 0; meshIndex.m_vertexStride = sizeof(btVector3); m_indexedMeshes.push_back(meshIndex); if (m_use32bitIndices) { - m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3; + m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size() / 3; m_indexedMeshes[0].m_triangleIndexBase = 0; m_indexedMeshes[0].m_indexType = PHY_INTEGER; - m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int); - } else + m_indexedMeshes[0].m_triangleIndexStride = 3 * sizeof(int); + } + else { - m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3; + m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size() / 3; m_indexedMeshes[0].m_triangleIndexBase = 0; m_indexedMeshes[0].m_indexType = PHY_SHORT; - m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int); + m_indexedMeshes[0].m_triangleIndexStride = 3 * sizeof(short int); } if (m_use4componentVertices) @@ -52,48 +50,48 @@ m_weldingThreshold(0.0) m_indexedMeshes[0].m_numVertices = m_4componentVertices.size(); m_indexedMeshes[0].m_vertexBase = 0; m_indexedMeshes[0].m_vertexStride = sizeof(btVector3); - } else + } + else { - m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3; + m_indexedMeshes[0].m_numVertices = m_3componentVertices.size() / 3; m_indexedMeshes[0].m_vertexBase = 0; - m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar); + m_indexedMeshes[0].m_vertexStride = 3 * sizeof(btScalar); } - - } -void btTriangleMesh::addIndex(int index) +void btTriangleMesh::addIndex(int index) { if (m_use32bitIndices) { m_32bitIndices.push_back(index); - m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0]; - } else + m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*)&m_32bitIndices[0]; + } + else { m_16bitIndices.push_back(index); - m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0]; + m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*)&m_16bitIndices[0]; } } -void btTriangleMesh::addTriangleIndices(int index1, int index2, int index3 ) +void btTriangleMesh::addTriangleIndices(int index1, int index2, int index3) { m_indexedMeshes[0].m_numTriangles++; - addIndex( index1 ); - addIndex( index2 ); - addIndex( index3 ); + addIndex(index1); + addIndex(index2); + addIndex(index3); } -int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices) +int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices) { //return index of new/existing vertex ///@todo: could use acceleration structure for this if (m_use4componentVertices) { if (removeDuplicateVertices) + { + for (int i = 0; i < m_4componentVertices.size(); i++) { - for (int i=0;i< m_4componentVertices.size();i++) - { - if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold) + if ((m_4componentVertices[i] - vertex).length2() <= m_weldingThreshold) { return i; } @@ -103,19 +101,18 @@ int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicat m_4componentVertices.push_back(vertex); m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0]; - return m_4componentVertices.size()-1; - - } else + return m_4componentVertices.size() - 1; + } + else { - if (removeDuplicateVertices) { - for (int i=0;i< m_3componentVertices.size();i+=3) + for (int i = 0; i < m_3componentVertices.size(); i += 3) { - btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]); - if ((vtx-vertex).length2() <= m_weldingThreshold) + btVector3 vtx(m_3componentVertices[i], m_3componentVertices[i + 1], m_3componentVertices[i + 2]); + if ((vtx - vertex).length2() <= m_weldingThreshold) { - return i/3; + return i / 3; } } } @@ -124,17 +121,16 @@ int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicat m_3componentVertices.push_back(vertex.getZ()); m_indexedMeshes[0].m_numVertices++; m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0]; - return (m_3componentVertices.size()/3)-1; + return (m_3componentVertices.size() / 3) - 1; } - } - -void btTriangleMesh::addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2,bool removeDuplicateVertices) + +void btTriangleMesh::addTriangle(const btVector3& vertex0, const btVector3& vertex1, const btVector3& vertex2, bool removeDuplicateVertices) { m_indexedMeshes[0].m_numTriangles++; - addIndex(findOrAddVertex(vertex0,removeDuplicateVertices)); - addIndex(findOrAddVertex(vertex1,removeDuplicateVertices)); - addIndex(findOrAddVertex(vertex2,removeDuplicateVertices)); + addIndex(findOrAddVertex(vertex0, removeDuplicateVertices)); + addIndex(findOrAddVertex(vertex1, removeDuplicateVertices)); + addIndex(findOrAddVertex(vertex2, removeDuplicateVertices)); } int btTriangleMesh::getNumTriangles() const @@ -151,7 +147,8 @@ void btTriangleMesh::preallocateVertices(int numverts) if (m_use4componentVertices) { m_4componentVertices.reserve(numverts); - } else + } + else { m_3componentVertices.reserve(numverts); } @@ -162,7 +159,8 @@ void btTriangleMesh::preallocateIndices(int numindices) if (m_use32bitIndices) { m_32bitIndices.reserve(numindices); - } else + } + else { m_16bitIndices.reserve(numindices); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index ac4afa7f6b2..a8a362355cc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -26,47 +26,44 @@ subject to the following restrictions: ///Performance of btTriangleMesh and btTriangleIndexVertexArray used in a btBvhTriangleMeshShape is the same. class btTriangleMesh : public btTriangleIndexVertexArray { - btAlignedObjectArray<btVector3> m_4componentVertices; - btAlignedObjectArray<btScalar> m_3componentVertices; + btAlignedObjectArray<btVector3> m_4componentVertices; + btAlignedObjectArray<btScalar> m_3componentVertices; - btAlignedObjectArray<unsigned int> m_32bitIndices; - btAlignedObjectArray<unsigned short int> m_16bitIndices; - bool m_use32bitIndices; - bool m_use4componentVertices; - + btAlignedObjectArray<unsigned int> m_32bitIndices; + btAlignedObjectArray<unsigned short int> m_16bitIndices; + bool m_use32bitIndices; + bool m_use4componentVertices; - public: - btScalar m_weldingThreshold; +public: + btScalar m_weldingThreshold; - btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true); + btTriangleMesh(bool use32bitIndices = true, bool use4componentVertices = true); - bool getUse32bitIndices() const - { - return m_use32bitIndices; - } + bool getUse32bitIndices() const + { + return m_use32bitIndices; + } - bool getUse4componentVertices() const - { - return m_use4componentVertices; - } - ///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes. - ///In general it is better to directly use btTriangleIndexVertexArray instead. - void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2, bool removeDuplicateVertices=false); + bool getUse4componentVertices() const + { + return m_use4componentVertices; + } + ///By default addTriangle won't search for duplicate vertices, because the search is very slow for large triangle meshes. + ///In general it is better to directly use btTriangleIndexVertexArray instead. + void addTriangle(const btVector3& vertex0, const btVector3& vertex1, const btVector3& vertex2, bool removeDuplicateVertices = false); - ///Add a triangle using its indices. Make sure the indices are pointing within the vertices array, so add the vertices first (and to be sure, avoid removal of duplicate vertices) - void addTriangleIndices(int index1, int index2, int index3 ); - - int getNumTriangles() const; + ///Add a triangle using its indices. Make sure the indices are pointing within the vertices array, so add the vertices first (and to be sure, avoid removal of duplicate vertices) + void addTriangleIndices(int index1, int index2, int index3); - virtual void preallocateVertices(int numverts); - virtual void preallocateIndices(int numindices); + int getNumTriangles() const; - ///findOrAddVertex is an internal method, use addTriangle instead - int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices); - ///addIndex is an internal method, use addTriangle instead - void addIndex(int index); - -}; + virtual void preallocateVertices(int numverts); + virtual void preallocateIndices(int numindices); -#endif //BT_TRIANGLE_MESH_H + ///findOrAddVertex is an internal method, use addTriangle instead + int findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices); + ///addIndex is an internal method, use addTriangle instead + void addIndex(int index); +}; +#endif //BT_TRIANGLE_MESH_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp index 0e17951405c..aec239063cd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -20,12 +20,11 @@ subject to the following restrictions: #include "LinearMath/btAabbUtil2.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" - btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) -: btConcaveShape (), m_meshInterface(meshInterface) + : btConcaveShape(), m_meshInterface(meshInterface) { m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; - if(meshInterface->hasPremadeAabb()) + if (meshInterface->hasPremadeAabb()) { meshInterface->getPremadeAabb(&m_localAabbMin, &m_localAabbMax); } @@ -35,69 +34,60 @@ btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) } } - btTriangleMeshShape::~btTriangleMeshShape() { - } - - - -void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +void btTriangleMeshShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { + btVector3 localHalfExtents = btScalar(0.5) * (m_localAabbMax - m_localAabbMin); + localHalfExtents += btVector3(getMargin(), getMargin(), getMargin()); + btVector3 localCenter = btScalar(0.5) * (m_localAabbMax + m_localAabbMin); - btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); - localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); - btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); - - btMatrix3x3 abs_b = trans.getBasis().absolute(); + btMatrix3x3 abs_b = trans.getBasis().absolute(); btVector3 center = trans(localCenter); - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); + btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); aabbMin = center - extent; aabbMax = center + extent; } -void btTriangleMeshShape::recalcLocalAabb() +void btTriangleMeshShape::recalcLocalAabb() { - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]+m_collisionMargin; + m_localAabbMax[i] = tmp[i] + m_collisionMargin; vec[i] = btScalar(-1.); tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]-m_collisionMargin; + m_localAabbMin[i] = tmp[i] - m_collisionMargin; } } - - class SupportVertexCallback : public btTriangleCallback { - btVector3 m_supportVertexLocal; -public: - btTransform m_worldTrans; +public: + btTransform m_worldTrans; btScalar m_maxDot; btVector3 m_supportVecLocal; - SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans) - : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-BT_LARGE_FLOAT)) - + SupportVertexCallback(const btVector3& supportVecWorld, const btTransform& trans) + : m_supportVertexLocal(btScalar(0.), btScalar(0.), btScalar(0.)), m_worldTrans(trans), m_maxDot(btScalar(-BT_LARGE_FLOAT)) + { m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis(); } - virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex) + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { (void)partId; (void)triangleIndex; - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { btScalar dot = m_supportVecLocal.dot(triangle[i]); if (dot > m_maxDot) @@ -113,14 +103,12 @@ public: return m_worldTrans(m_supportVertexLocal); } - btVector3 GetSupportVertexLocal() + btVector3 GetSupportVertexLocal() { return m_supportVertexLocal; } - }; - void btTriangleMeshShape::setLocalScaling(const btVector3& scaling) { m_meshInterface->setScaling(scaling); @@ -132,60 +120,46 @@ const btVector3& btTriangleMeshShape::getLocalScaling() const return m_meshInterface->getScaling(); } - - - - - //#define DEBUG_TRIANGLE_MESH - - -void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) const { - struct FilteredCallback : public btInternalTriangleIndexCallback + struct FilteredCallback : public btInternalTriangleIndexCallback { btTriangleCallback* m_callback; btVector3 m_aabbMin; btVector3 m_aabbMax; - FilteredCallback(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) - :m_callback(callback), - m_aabbMin(aabbMin), - m_aabbMax(aabbMax) + FilteredCallback(btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax) + : m_callback(callback), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) { } - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) { - if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax)) + if (TestTriangleAgainstAabb2(&triangle[0], m_aabbMin, m_aabbMax)) { //check aabb in triangle-space, before doing this - m_callback->processTriangle(triangle,partId,triangleIndex); + m_callback->processTriangle(triangle, partId, triangleIndex); } - } - }; - FilteredCallback filterCallback(callback,aabbMin,aabbMax); + FilteredCallback filterCallback(callback, aabbMin, aabbMax); - m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax); + m_meshInterface->InternalProcessAllTriangles(&filterCallback, aabbMin, aabbMax); } - - - - -void btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btTriangleMeshShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { (void)mass; //moving concave objects not supported btAssert(0); - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } - btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 supportVertex; @@ -193,15 +167,13 @@ btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) co btTransform ident; ident.setIdentity(); - SupportVertexCallback supportCallback(vec,ident); + SupportVertexCallback supportCallback(vec, ident); + + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + + processAllTriangles(&supportCallback, -aabbMax, aabbMax); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - - processAllTriangles(&supportCallback,-aabbMax,aabbMax); - supportVertex = supportCallback.GetSupportVertexLocal(); return supportVertex; } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index 453e58005a4..4a70e283fae 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -19,18 +19,18 @@ subject to the following restrictions: #include "btConcaveShape.h" #include "btStridingMeshInterface.h" - ///The btTriangleMeshShape is an internal concave triangle mesh interface. Don't use this class directly, use btBvhTriangleMeshShape instead. -ATTRIBUTE_ALIGNED16(class) btTriangleMeshShape : public btConcaveShape +ATTRIBUTE_ALIGNED16(class) +btTriangleMeshShape : public btConcaveShape { protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; btStridingMeshInterface* m_meshInterface; ///btTriangleMeshShape constructor has been disabled/protected, so that users will not mistakenly use this class. ///Don't use btTriangleMeshShape but use btBvhTriangleMeshShape instead! - btTriangleMeshShape(btStridingMeshInterface* meshInterface); + btTriangleMeshShape(btStridingMeshInterface * meshInterface); public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -39,23 +39,23 @@ public: virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { btAssert(0); return localGetSupportingVertex(vec); } - void recalcLocalAabb(); + void recalcLocalAabb(); - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + virtual void processAllTriangles(btTriangleCallback * callback, const btVector3& aabbMin, const btVector3& aabbMax) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - virtual void setLocalScaling(const btVector3& scaling); + virtual void setLocalScaling(const btVector3& scaling); virtual const btVector3& getLocalScaling() const; - + btStridingMeshInterface* getMeshInterface() { return m_meshInterface; @@ -75,16 +75,8 @@ public: return m_localAabbMax; } - - //debugging - virtual const char* getName()const {return "TRIANGLEMESH";} - - - + virtual const char* getName() const { return "TRIANGLEMESH"; } }; - - - -#endif //BT_TRIANGLE_MESH_SHAPE_H +#endif //BT_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h index a8a80f82fe2..190cbdae69e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -19,15 +19,13 @@ subject to the following restrictions: #include "btConvexShape.h" #include "btBoxShape.h" -ATTRIBUTE_ALIGNED16(class) btTriangleShape : public btPolyhedralConvexShape +ATTRIBUTE_ALIGNED16(class) +btTriangleShape : public btPolyhedralConvexShape { - - public: + BT_DECLARE_ALIGNED_ALLOCATOR(); -BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 m_vertices1[3]; + btVector3 m_vertices1[3]; virtual int getNumVertices() const { @@ -43,7 +41,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { return m_vertices1[index]; } - virtual void getVertex(int index,btVector3& vert) const + virtual void getVertex(int index, btVector3& vert) const { vert = m_vertices1[index]; } @@ -52,83 +50,79 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { return 3; } - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const + + virtual void getEdge(int i, btVector3& pa, btVector3& pb) const { - getVertex(i,pa); - getVertex((i+1)%3,pb); + getVertex(i, pa); + getVertex((i + 1) % 3, pb); } - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { -// btAssert(0); - getAabbSlow(t,aabbMin,aabbMax); + // btAssert(0); + getAabbSlow(t, aabbMin, aabbMax); } - btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir)const + btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir) const { - btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]); - return m_vertices1[dots.maxAxis()]; - + btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]); + return m_vertices1[dots.maxAxis()]; } - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - for (int i=0;i<numVectors;i++) + for (int i = 0; i < numVectors; i++) { const btVector3& dir = vectors[i]; - btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]); - supportVerticesOut[i] = m_vertices1[dots.maxAxis()]; + btVector3 dots = dir.dot3(m_vertices1[0], m_vertices1[1], m_vertices1[2]); + supportVerticesOut[i] = m_vertices1[dots.maxAxis()]; } - } - btTriangleShape() : btPolyhedralConvexShape () - { + btTriangleShape() : btPolyhedralConvexShape() + { m_shapeType = TRIANGLE_SHAPE_PROXYTYPE; } - btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) : btPolyhedralConvexShape () - { + btTriangleShape(const btVector3& p0, const btVector3& p1, const btVector3& p2) : btPolyhedralConvexShape() + { m_shapeType = TRIANGLE_SHAPE_PROXYTYPE; - m_vertices1[0] = p0; - m_vertices1[1] = p1; - m_vertices1[2] = p2; - } - + m_vertices1[0] = p0; + m_vertices1[1] = p1; + m_vertices1[2] = p2; + } - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i) const + virtual void getPlane(btVector3 & planeNormal, btVector3 & planeSupport, int i) const { - getPlaneEquation(i,planeNormal,planeSupport); + getPlaneEquation(i, planeNormal, planeSupport); } - virtual int getNumPlanes() const + virtual int getNumPlanes() const { return 1; } - void calcNormal(btVector3& normal) const + void calcNormal(btVector3 & normal) const { - normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]); + normal = (m_vertices1[1] - m_vertices1[0]).cross(m_vertices1[2] - m_vertices1[0]); normal.normalize(); } - virtual void getPlaneEquation(int i, btVector3& planeNormal,btVector3& planeSupport) const + virtual void getPlaneEquation(int i, btVector3& planeNormal, btVector3& planeSupport) const { (void)i; calcNormal(planeNormal); planeSupport = m_vertices1[0]; } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const { (void)mass; btAssert(0); - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + inertia.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } - virtual bool isInside(const btVector3& pt,btScalar tolerance) const + virtual bool isInside(const btVector3& pt, btScalar tolerance) const { btVector3 normal; calcNormal(normal); @@ -140,45 +134,42 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { //inside check on edge-planes int i; - for (i=0;i<3;i++) + for (i = 0; i < 3; i++) { - btVector3 pa,pb; - getEdge(i,pa,pb); - btVector3 edge = pb-pa; + btVector3 pa, pb; + getEdge(i, pa, pb); + btVector3 edge = pb - pa; btVector3 edgeNormal = edge.cross(normal); edgeNormal.normalize(); - btScalar dist = pt.dot( edgeNormal); + btScalar dist = pt.dot(edgeNormal); btScalar edgeConst = pa.dot(edgeNormal); dist -= edgeConst; if (dist < -tolerance) return false; } - + return true; } return false; } - //debugging - virtual const char* getName()const - { - return "Triangle"; - } - - virtual int getNumPreferredPenetrationDirections() const - { - return 2; - } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const - { - calcNormal(penetrationVector); - if (index) - penetrationVector *= btScalar(-1.); - } + //debugging + virtual const char* getName() const + { + return "Triangle"; + } + virtual int getNumPreferredPenetrationDirections() const + { + return 2; + } + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + { + calcNormal(penetrationVector); + if (index) + penetrationVector *= btScalar(-1.); + } }; -#endif //BT_OBB_TRIANGLE_MINKOWSKI_H - +#endif //BT_OBB_TRIANGLE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp index b148bbd99a5..ed3cd2d2597 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp @@ -15,122 +15,112 @@ subject to the following restrictions: #include "btUniformScalingShape.h" -btUniformScalingShape::btUniformScalingShape( btConvexShape* convexChildShape,btScalar uniformScalingFactor): -btConvexShape (), m_childConvexShape(convexChildShape), -m_uniformScalingFactor(uniformScalingFactor) +btUniformScalingShape::btUniformScalingShape(btConvexShape* convexChildShape, btScalar uniformScalingFactor) : btConvexShape(), m_childConvexShape(convexChildShape), m_uniformScalingFactor(uniformScalingFactor) { m_shapeType = UNIFORM_SCALING_SHAPE_PROXYTYPE; } - + btUniformScalingShape::~btUniformScalingShape() { } - -btVector3 btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +btVector3 btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const { btVector3 tmpVertex; tmpVertex = m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); - return tmpVertex*m_uniformScalingFactor; + return tmpVertex * m_uniformScalingFactor; } -void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const { - m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); + m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors, supportVerticesOut, numVectors); int i; - for (i=0;i<numVectors;i++) + for (i = 0; i < numVectors; i++) { supportVerticesOut[i] = supportVerticesOut[i] * m_uniformScalingFactor; } } - -btVector3 btUniformScalingShape::localGetSupportingVertex(const btVector3& vec)const +btVector3 btUniformScalingShape::localGetSupportingVertex(const btVector3& vec) const { btVector3 tmpVertex; tmpVertex = m_childConvexShape->localGetSupportingVertex(vec); - return tmpVertex*m_uniformScalingFactor; + return tmpVertex * m_uniformScalingFactor; } - -void btUniformScalingShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +void btUniformScalingShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const { - ///this linear upscaling is not realistic, but we don't deal with large mass ratios... btVector3 tmpInertia; - m_childConvexShape->calculateLocalInertia(mass,tmpInertia); + m_childConvexShape->calculateLocalInertia(mass, tmpInertia); inertia = tmpInertia * m_uniformScalingFactor; } - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btUniformScalingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btUniformScalingShape::getAabb(const btTransform& trans, btVector3& aabbMin, btVector3& aabbMax) const { - getAabbSlow(trans,aabbMin,aabbMax); - + getAabbSlow(trans, aabbMin, aabbMax); } -void btUniformScalingShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +void btUniformScalingShape::getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { #if 1 btVector3 _directions[] = - { - btVector3( 1., 0., 0.), - btVector3( 0., 1., 0.), - btVector3( 0., 0., 1.), - btVector3( -1., 0., 0.), - btVector3( 0., -1., 0.), - btVector3( 0., 0., -1.) - }; - + { + btVector3(1., 0., 0.), + btVector3(0., 1., 0.), + btVector3(0., 0., 1.), + btVector3(-1., 0., 0.), + btVector3(0., -1., 0.), + btVector3(0., 0., -1.)}; + btVector3 _supporting[] = + { + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.), + btVector3(0., 0., 0.)}; + + for (int i = 0; i < 6; i++) { - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.) - }; - - for (int i=0;i<6;i++) - { - _directions[i] = _directions[i]*t.getBasis(); + _directions[i] = _directions[i] * t.getBasis(); } - + batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); - - btVector3 aabbMin1(0,0,0),aabbMax1(0,0,0); - for ( int i = 0; i < 3; ++i ) + btVector3 aabbMin1(0, 0, 0), aabbMax1(0, 0, 0); + + for (int i = 0; i < 3; ++i) { aabbMax1[i] = t(_supporting[i])[i]; aabbMin1[i] = t(_supporting[i + 3])[i]; } - btVector3 marginVec(getMargin(),getMargin(),getMargin()); - aabbMin = aabbMin1-marginVec; - aabbMax = aabbMax1+marginVec; - + btVector3 marginVec(getMargin(), getMargin(), getMargin()); + aabbMin = aabbMin1 - marginVec; + aabbMax = aabbMax1 + marginVec; + #else btScalar margin = getMargin(); - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 vec(btScalar(0.), btScalar(0.), btScalar(0.)); vec[i] = btScalar(1.); - btVector3 sv = localGetSupportingVertex(vec*t.getBasis()); + btVector3 sv = localGetSupportingVertex(vec * t.getBasis()); btVector3 tmp = t(sv); - aabbMax[i] = tmp[i]+margin; + aabbMax[i] = tmp[i] + margin; vec[i] = btScalar(-1.); - sv = localGetSupportingVertex(vec*t.getBasis()); + sv = localGetSupportingVertex(vec * t.getBasis()); tmp = t(sv); - aabbMin[i] = tmp[i]-margin; + aabbMin[i] = tmp[i] - margin; } #endif } -void btUniformScalingShape::setLocalScaling(const btVector3& scaling) +void btUniformScalingShape::setLocalScaling(const btVector3& scaling) { m_childConvexShape->setLocalScaling(scaling); } @@ -140,21 +130,21 @@ const btVector3& btUniformScalingShape::getLocalScaling() const return m_childConvexShape->getLocalScaling(); } -void btUniformScalingShape::setMargin(btScalar margin) +void btUniformScalingShape::setMargin(btScalar margin) { m_childConvexShape->setMargin(margin); } -btScalar btUniformScalingShape::getMargin() const +btScalar btUniformScalingShape::getMargin() const { return m_childConvexShape->getMargin() * m_uniformScalingFactor; } -int btUniformScalingShape::getNumPreferredPenetrationDirections() const +int btUniformScalingShape::getNumPreferredPenetrationDirections() const { return m_childConvexShape->getNumPreferredPenetrationDirections(); } - -void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const + +void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const { - m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); + m_childConvexShape->getPreferredPenetrationDirection(index, penetrationVector); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h index a10f58d2429..4dfe34efbdc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h @@ -17,73 +17,68 @@ subject to the following restrictions: #define BT_UNIFORM_SCALING_SHAPE_H #include "btConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types ///The btUniformScalingShape allows to re-use uniform scaled instances of btConvexShape in a memory efficient way. ///Istead of using btUniformScalingShape, it is better to use the non-uniform setLocalScaling method on convex shapes that implement it. -ATTRIBUTE_ALIGNED16(class) btUniformScalingShape : public btConvexShape +ATTRIBUTE_ALIGNED16(class) +btUniformScalingShape : public btConvexShape { - btConvexShape* m_childConvexShape; + btConvexShape* m_childConvexShape; - btScalar m_uniformScalingFactor; - - public: - + btScalar m_uniformScalingFactor; + +public: BT_DECLARE_ALIGNED_ALLOCATOR(); - - btUniformScalingShape( btConvexShape* convexChildShape, btScalar uniformScalingFactor); - + + btUniformScalingShape(btConvexShape * convexChildShape, btScalar uniformScalingFactor); + virtual ~btUniformScalingShape(); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const; + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual void calculateLocalInertia(btScalar mass, btVector3 & inertia) const; - btScalar getUniformScalingFactor() const + btScalar getUniformScalingFactor() const { return m_uniformScalingFactor; } - btConvexShape* getChildShape() + btConvexShape* getChildShape() { return m_childConvexShape; } - const btConvexShape* getChildShape() const + const btConvexShape* getChildShape() const { return m_childConvexShape; } - virtual const char* getName()const + virtual const char* getName() const { return "UniformScalingShape"; } - - /////////////////////////// - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version - void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling) ; - virtual const btVector3& getLocalScaling() const ; + virtual void getAabbSlow(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - virtual void setMargin(btScalar margin); - virtual btScalar getMargin() const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; - virtual int getNumPreferredPenetrationDirections() const; - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + virtual int getNumPreferredPenetrationDirections() const; + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; }; -#endif //BT_UNIFORM_SCALING_SHAPE_H +#endif //BT_UNIFORM_SCALING_SHAPE_H |