/********************************************************************************
* 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 <complex>
#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) {
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.
/// If you use this constructor, you will need to set the vertices manually one by one using
/// the addVertex() method.
ConvexMeshShape::ConvexMeshShape(decimal margin)
: CollisionShape(CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
}
// 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) {
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,
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];
}
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<mNbVertices; i++) {
// Compute the dot product of the current vertex
decimal 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];
}
}
// Recompute the bounds of the mesh
void ConvexMeshShape::recalculateBounds() {
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);
}
// Test equality between two cone shapes
bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
const ConvexMeshShape& otherShape = dynamic_cast<const ConvexMeshShape&>(otherCollisionShape);
assert(mNbVertices == mVertices.size());
if (mNbVertices != otherShape.mNbVertices) return false;
if (mIsEdgesInformationUsed != otherShape.mIsEdgesInformationUsed) return false;
if (mEdgesAdjacencyList.size() != otherShape.mEdgesAdjacencyList.size()) return false;
// Check that the vertices are the same
for (uint i=0; i<mNbVertices; i++) {
if (mVertices[i] != otherShape.mVertices[i]) return false;
}
// Check that the edges are the same
for (uint i=0; i<mEdgesAdjacencyList.size(); i++) {
assert(otherShape.mEdgesAdjacencyList.count(i) == 1);
if (mEdgesAdjacencyList.at(i) != otherShape.mEdgesAdjacencyList.at(i)) return false;
}
return true;
}
// Raycast method
bool ConvexMeshShape::raycast(const Ray& ray, decimal distance) const {
// TODO : Implement this method
return false;
}
// Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, decimal distance) const {
// TODO : Implement this method
return false;
}