/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 <complex>
#include "configuration.h"
#include "ConvexMeshShape.h"
using namespace reactphysics3d;
// Constructor to initialize with an array of 3D vertices.
/// This method creates an internal copy of the input vertices.
/**
* @param arrayVertices Array with the vertices of the convex mesh
* @param nbVertices Number of vertices in the convex mesh
* @param stride Stride between the beginning of two elements in the vertices array
* @param margin Collision margin (in meters) around the collision shape
*/
ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride,
decimal margin)
: ConvexShape(CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
assert(nbVertices > 0);
assert(stride > 0);
const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;
// Copy all the vertices into the internal array
for (uint i=0; i<mNbVertices; i++) {
const decimal* newPoint = (const decimal*) vertexPointer;
mVertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2]));
vertexPointer += stride;
}
// Recalculate the bounds of the mesh
recalculateBounds();
}
// Constructor to initialize with a triangle mesh
/// This method creates an internal copy of the input vertices.
/**
* @param triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh
* @param isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory)
* @param margin Collision margin (in meters) around the collision shape
*/
ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed, decimal margin)
: ConvexShape(CONVEX_MESH, margin), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(isEdgesInformationUsed) {
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
// For each vertex of the mesh
for (int v = 0; v < triangleVertexArray->getNbVertices(); v++) {
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + v * vertexStride);
Vector3 vertex(vertices[0], vertices[1], vertices[2] );
vertex = vertex * mScaling;
mVertices.push_back(vertex);
}
else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + v * vertexStride);
Vector3 vertex(vertices[0], vertices[1], vertices[2] );
vertex = vertex * mScaling;
mVertices.push_back(vertex);
}
}
// If we need to use the edges information of the mesh
if (mIsEdgesInformationUsed) {
// For each triangle of the mesh
for (int triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
uint vertexIndex[3];
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex[k] = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k];
}
}
// Add information about the edges
addEdge(vertexIndex[0], vertexIndex[1]);
addEdge(vertexIndex[0], vertexIndex[2]);
addEdge(vertexIndex[1], vertexIndex[2]);
}
}
mNbVertices = mVertices.size();
recalculateBounds();
}
// Constructor.
/// If you use this constructor, you will need to set the vertices manually one by one using
/// the addVertex() method.
ConvexMeshShape::ConvexMeshShape(decimal margin)
: ConvexShape(CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
}
// Destructor
ConvexMeshShape::~ConvexMeshShape() {
}
// Return a local support point in a given direction with the object margin
Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Get the support point without the margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
// 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,
void** cachedCollisionData) const {
assert(mNbVertices == mVertices.size());
assert(cachedCollisionData != NULL);
// Allocate memory for the cached collision data if not allocated yet
if ((*cachedCollisionData) == NULL) {
*cachedCollisionData = (int*) malloc(sizeof(int));
*((int*)(*cachedCollisionData)) = 0;
}
// If the edges information is used to speed up the collision detection
if (mIsEdgesInformationUsed) {
assert(mEdgesAdjacencyList.size() == mNbVertices);
uint maxVertex = *((int*)(*cachedCollisionData));
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<uint>::const_iterator it;
std::set<uint>::const_iterator itBegin = mEdgesAdjacencyList.at(maxVertex).begin();
std::set<uint>::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
*((int*)(*cachedCollisionData)) = maxVertex;
// Return the support vertex
return mVertices[maxVertex] * mScaling;
}
else { // If the edges information is not used
double maxDotProduct = DECIMAL_SMALLEST;
uint indexMaxDotProduct = 0;
// For each vertex of the mesh
for (uint i=0; i<mNbVertices; i++) {
// Compute the dot product of the current vertex
double dotProduct = direction.dot(mVertices[i]);
// If the current dot product is larger than the maximum one
if (dotProduct > 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] * mScaling;
}
}
// Recompute the bounds of the mesh
void ConvexMeshShape::recalculateBounds() {
// TODO : Only works if the local origin is inside the mesh
// => Make it more robust (init with first vertex of mesh instead)
mMinBounds.setToZero();
mMaxBounds.setToZero();
// For each vertex of the mesh
for (uint i=0; i<mNbVertices; i++) {
if (mVertices[i].x > 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);
}
// Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo);
}