diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp new file mode 100644 index 00000000..75f819bc --- /dev/null +++ b/src/collision/shapes/ConvexMeshShape.cpp @@ -0,0 +1,228 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include +#include "../../configuration.h" +#include "ConvexMeshShape.h" + +using namespace reactphysics3d; + +// Constructor to initialize with a array of 3D vertices. +/// This method creates an internal copy of the input vertices. +ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, + decimal margin) + : CollisionShape(CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0), + mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false), mCachedSupportVertex(0) { + assert(nbVertices > 0); + assert(stride > 0); + assert(margin > decimal(0.0)); + + const unsigned char* vertexPointer = (const unsigned char*) arrayVertices; + + // Copy all the vertices into the internal array + for (uint i=0; i decimal(0.0)); +} + +// Private copy-constructor +ConvexMeshShape::ConvexMeshShape(const ConvexMeshShape& shape) + : CollisionShape(shape), mVertices(shape.mVertices), mNbVertices(shape.mNbVertices), + mMinBounds(shape.mMinBounds), mMaxBounds(shape.mMaxBounds), + mIsEdgesInformationUsed(shape.mIsEdgesInformationUsed), + mEdgesAdjacencyList(shape.mEdgesAdjacencyList), + mCachedSupportVertex(shape.mCachedSupportVertex) { + + assert(mNbVertices == mVertices.size()); +} + +// Destructor +ConvexMeshShape::~ConvexMeshShape() { + +} + +// Return a local support point in a given direction with the object margin +Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction) { + + // Get the support point without the margin + Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction); + + // Get the unit direction vector + Vector3 unitDirection = direction; + if (direction.lengthSquare() < MACHINE_EPSILON * MACHINE_EPSILON) { + unitDirection.setAllValues(1.0, 1.0, 1.0); + } + unitDirection.normalize(); + + // Add the margin to the support point and return it + return supportPoint + unitDirection * mMargin; +} + +// Return a local support point in a given direction without the object margin. +/// If the edges information is not used for collision detection, this method will go through +/// the whole vertices list and pick up the vertex with the largest dot product in the support +/// direction. This is an O(n) process with "n" being the number of vertices in the mesh. +/// However, if the edges information is used, we can cache the previous support vertex and use +/// it as a start in a hill-climbing (local search) process to find the new support vertex which +/// will be in most of the cases very close to the previous one. Using hill-climbing, this method +/// runs in almost constant time. +Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) { + + assert(mNbVertices == mVertices.size()); + + // If the edges information is used to speed up the collision detection + if (mIsEdgesInformationUsed) { + + assert(mEdgesAdjacencyList.size() == mNbVertices); + + uint maxVertex = mCachedSupportVertex; + decimal maxDotProduct = direction.dot(mVertices[maxVertex]); + bool isOptimal; + + // Perform hill-climbing (local search) + do { + isOptimal = true; + + assert(mEdgesAdjacencyList.at(maxVertex).size() > 0); + + // For all neighbors of the current vertex + std::set::const_iterator it; + std::set::const_iterator itBegin = mEdgesAdjacencyList.at(maxVertex).begin(); + std::set::const_iterator itEnd = mEdgesAdjacencyList.at(maxVertex).end(); + for (it = itBegin; it != itEnd; ++it) { + + // Compute the dot product + decimal dotProduct = direction.dot(mVertices[*it]); + + // If the current vertex is a better vertex (larger dot product) + if (dotProduct > maxDotProduct) { + maxVertex = *it; + maxDotProduct = dotProduct; + isOptimal = false; + } + } + + } while(!isOptimal); + + // Cache the support vertex + mCachedSupportVertex = maxVertex; + + // Return the support vertex + return mVertices[maxVertex]; + } + else { // If the edges information is not used + + decimal maxDotProduct = DECIMAL_SMALLEST; + uint indexMaxDotProduct = 0; + + // For each vertex of the mesh + for (uint i=0; i maxDotProduct) { + indexMaxDotProduct = i; + maxDotProduct = dotProduct; + } + } + + assert(maxDotProduct >= decimal(0.0)); + + // Return the vertex with the largest dot product in the support direction + return mVertices[indexMaxDotProduct]; + } +} + +// Recompute the bounds of the mesh +void ConvexMeshShape::recalculateBounds() { + + mMinBounds.setToZero(); + mMaxBounds.setToZero(); + + // For each vertex of the mesh + for (uint i=0; i mMaxBounds.x) mMaxBounds.x = mVertices[i].x; + if (mVertices[i].x < mMinBounds.x) mMinBounds.x = mVertices[i].x; + + if (mVertices[i].y > mMaxBounds.y) mMaxBounds.y = mVertices[i].y; + if (mVertices[i].y < mMinBounds.y) mMinBounds.y = mVertices[i].y; + + if (mVertices[i].z > mMaxBounds.z) mMaxBounds.z = mVertices[i].z; + if (mVertices[i].z < mMinBounds.z) mMinBounds.z = mVertices[i].z; + } + + // Add the object margin to the bounds + mMaxBounds += Vector3(mMargin, mMargin, mMargin); + mMinBounds -= Vector3(mMargin, mMargin, mMargin); +} + +// Test equality between two cone shapes +bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const { + const ConvexMeshShape& otherShape = dynamic_cast(otherCollisionShape); + + assert(mNbVertices == mVertices.size()); + + if (mNbVertices != otherShape.mNbVertices) return false; + + // If edges information is used, it means that a collison shape object will store + // cached data (previous support vertex) and therefore, we should not reuse the shape + // for another body. Therefore, we consider that all convex mesh shape using edges + // information are different. + if (mIsEdgesInformationUsed) return false; + + if (mEdgesAdjacencyList.size() != otherShape.mEdgesAdjacencyList.size()) return false; + + // Check that the vertices are the same + for (uint i=0; i +#include +#include + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Class ConvexMeshShape +/** + * This class represents a convex mesh shape. In order to create a convex mesh shape, you + * need to indicate the local-space position of the mesh vertices. You do it either by + * passing a vertices array to the constructor or using the addVertex() method. Make sure + * that the set of vertices that you use to create the shape are indeed part of a convex + * mesh. The center of mass of the shape will be at the origin of the local-space geometry + * that you use to create the mesh. The method used for collision detection with a convex + * mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh. + * Therefore, you should try not to use too many vertices. However, it is possible to speed + * up the collision detection by using the edges information of your mesh. The running time + * of the collision detection that uses the edges is almost O(1) constant time at the cost + * of additional memory used to store the vertices. You can indicate edges information + * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method + * in order to use the edges information for collision detection. + */ +class ConvexMeshShape : public CollisionShape { + + private : + + // -------------------- Attributes -------------------- // + + /// Array with the vertices of the mesh + std::vector mVertices; + + /// Number of vertices in the mesh + uint mNbVertices; + + /// Mesh minimum bounds in the three local x, y and z directions + Vector3 mMinBounds; + + /// Mesh maximum bounds in the three local x, y and z directions + Vector3 mMaxBounds; + + /// True if the shape contains the edges of the convex mesh in order to + /// make the collision detection faster + bool mIsEdgesInformationUsed; + + /// Adjacency list representing the edges of the mesh + std::map > mEdgesAdjacencyList; + + /// Cached support vertex index (previous support vertex) + uint mCachedSupportVertex; + + // -------------------- Methods -------------------- // + + /// Private copy-constructor + ConvexMeshShape(const ConvexMeshShape& shape); + + /// Private assignment operator + ConvexMeshShape& operator=(const ConvexMeshShape& shape); + + /// Recompute the bounds of the mesh + void recalculateBounds(); + + public : + + // -------------------- Methods -------------------- // + + /// Constructor to initialize with a array of 3D vertices. + ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, + decimal margin = OBJECT_MARGIN); + + /// Constructor. + ConvexMeshShape(decimal margin = OBJECT_MARGIN); + + /// Destructor + virtual ~ConvexMeshShape(); + + /// Allocate and return a copy of the object + virtual ConvexMeshShape* clone(void* allocatedMemory) const; + + /// Return the number of bytes used by the collision shape + virtual size_t getSizeInBytes() const; + + /// Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction); + + /// Return a local support point in a given direction without the object margin. + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction); + + /// Return the local bounds of the shape in x, y and z directions + virtual void getLocalBounds(Vector3& min, Vector3& max) const; + + /// Return the local inertia tensor of the collision shape. + virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + + /// Test equality between two cone shapes + virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const; + + /// Add a vertex into the convex mesh + void addVertex(const Vector3& vertex); + + /// Add an edge into the convex mesh by specifying the two vertex indices of the edge. + void addEdge(uint v1, uint v2); + + /// Return true if the edges information is used to speed up the collision detection + bool isEdgesInformationUsed() const; + + /// Set the variable to know if the edges information is used to speed up the + /// collision detection + void setIsEdgesInformationUsed(bool isEdgesUsed); +}; + +// Allocate and return a copy of the object +inline ConvexMeshShape* ConvexMeshShape::clone(void* allocatedMemory) const { + return new (allocatedMemory) ConvexMeshShape(*this); +} + +// Return the number of bytes used by the collision shape +inline size_t ConvexMeshShape::getSizeInBytes() const { + return sizeof(ConvexMeshShape); +} + +// Return the local bounds of the shape in x, y and z directions +inline void ConvexMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { + min = mMinBounds; + max = mMaxBounds; +} + +// Return the local inertia tensor of the collision shape. +/// The local inertia tensor of the convex mesh is approximated using the inertia tensor +/// of its bounding box. +inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { + decimal factor = (decimal(1.0) / decimal(3.0)) * mass; + Vector3 realExtent = decimal(0.5) * (mMaxBounds - mMinBounds); + assert(realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0); + decimal xSquare = realExtent.x * realExtent.x; + decimal ySquare = realExtent.y * realExtent.y; + decimal zSquare = realExtent.z * realExtent.z; + tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0, + 0.0, factor * (xSquare + zSquare), 0.0, + 0.0, 0.0, factor * (xSquare + ySquare)); +} + +// Add a vertex into the convex mesh +inline void ConvexMeshShape::addVertex(const Vector3& vertex) { + + // Add the vertex in to vertices array + mVertices.push_back(vertex); + mNbVertices++; + + // Update the bounds of the mesh + if (vertex.x > mMaxBounds.x) mMaxBounds.x = vertex.x; + if (vertex.x < mMinBounds.x) mMinBounds.x = vertex.x; + if (vertex.y > mMaxBounds.y) mMaxBounds.y = vertex.y; + if (vertex.y < mMinBounds.y) mMinBounds.y = vertex.y; + if (vertex.z > mMaxBounds.z) mMaxBounds.z = vertex.z; + if (vertex.z < mMinBounds.z) mMinBounds.z = vertex.z; +} + +// Add an edge into the convex mesh by specifying the two vertex indices of the edge. +/// Note that the vertex indices start at zero and need to correspond to the order of +/// the vertices in the vertices array in the constructor or the order of the calls +/// of the addVertex() methods that you use to add vertices into the convex mesh. +inline void ConvexMeshShape::addEdge(uint v1, uint v2) { + + assert(v1 >= 0); + assert(v2 >= 0); + + // If the entry for vertex v1 does not exist in the adjacency list + if (mEdgesAdjacencyList.count(v1) == 0) { + mEdgesAdjacencyList.insert(std::make_pair >(v1, std::set())); + } + + // If the entry for vertex v2 does not exist in the adjacency list + if (mEdgesAdjacencyList.count(v2) == 0) { + mEdgesAdjacencyList.insert(std::make_pair >(v2, std::set())); + } + + // Add the edge in the adjacency list + mEdgesAdjacencyList[v1].insert(v2); + mEdgesAdjacencyList[v2].insert(v1); +} + +// Return true if the edges information is used to speed up the collision detection +inline bool ConvexMeshShape::isEdgesInformationUsed() const { + return mIsEdgesInformationUsed; +} + +// Set the variable to know if the edges information is used to speed up the +// collision detection +inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { + mIsEdgesInformationUsed = isEdgesUsed; +} + +} + +#endif diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 2bf3885c..ababb9bc 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -48,6 +48,7 @@ #include "collision/shapes/ConeShape.h" #include "collision/shapes/CylinderShape.h" #include "collision/shapes/CapsuleShape.h" +#include "collision/shapes/ConvexMeshShape.h" #include "collision/shapes/AABB.h" #include "constraint/BallAndSocketJoint.h" #include "constraint/SliderJoint.h"