707 lines
22 KiB
C++
707 lines
22 KiB
C++
/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
|
|
*
|
|
* 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.
|
|
*/
|
|
|
|
#include "dgCollisionConvexHull.h"
|
|
#include "dgBody.h"
|
|
#include "dgContact.h"
|
|
#include "dgMeshEffect.h"
|
|
#include "hpl1/engine/libraries/newton/core/dg.h"
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// Construction/Destruction
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
struct dgPlaneLocation : public dgPlane {
|
|
int m_index;
|
|
const dgConvexSimplexEdge *m_face;
|
|
};
|
|
|
|
dgCollisionConvexHull::dgCollisionConvexHull(dgMemoryAllocator *const allocator,
|
|
dgUnsigned32 signature, dgInt32 count, dgInt32 strideInBytes,
|
|
dgFloat32 tolerance, const dgFloat32 *vertexArray, const dgMatrix &matrix) : dgCollisionConvex(allocator, signature, matrix, m_convexHullCollision) {
|
|
m_faceCount = 0;
|
|
m_edgeCount = 0;
|
|
m_vertexCount = 0;
|
|
m_vertex = NULL;
|
|
m_simplex = NULL;
|
|
m_faceArray = NULL;
|
|
m_boundPlanesCount = 0;
|
|
|
|
m_rtti |= dgCollisionConvexHull_RTTI;
|
|
|
|
Create(count, strideInBytes, vertexArray, tolerance);
|
|
|
|
dgInt32 planeCount = 0;
|
|
dgPlaneLocation planesArray[1024];
|
|
const dgConvexSimplexEdge *const *faceArray = m_faceArray;
|
|
for (dgInt32 i = 0; i < m_faceCount; i++) {
|
|
const dgConvexSimplexEdge *const face = faceArray[i];
|
|
|
|
dgInt32 i0 = face->m_prev->m_vertex;
|
|
dgInt32 i1 = face->m_vertex;
|
|
dgInt32 i2 = face->m_next->m_vertex;
|
|
const dgBigVector p0(m_vertex[i0]);
|
|
const dgBigVector p1(m_vertex[i1]);
|
|
const dgBigVector p2(m_vertex[i2]);
|
|
|
|
dgBigVector normal1((p1 - p0) * (p2 - p0));
|
|
|
|
dgVector normal(
|
|
(m_vertex[i1] - m_vertex[i0]) * (m_vertex[i2] - m_vertex[i0]));
|
|
normal = normal.Scale(dgFloat32(1.0f) / dgSqrt(normal % normal));
|
|
dgInt32 add = 1;
|
|
for (dgInt32 j = 0; j < 3; j++) {
|
|
if (dgAbsf(normal[j]) > dgFloat32(0.98f)) {
|
|
add = 0;
|
|
}
|
|
}
|
|
|
|
if (add) {
|
|
for (dgInt32 j = 0; j < planeCount; j++) {
|
|
dgFloat32 coplanar;
|
|
coplanar = normal % planesArray[j];
|
|
if (coplanar > 0.98f) {
|
|
add = 0;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (add) {
|
|
dgPlane plane(normal, dgFloat32(0.0f));
|
|
dgVector planeSupport(SupportVertex(plane));
|
|
plane.m_w = -(plane % planeSupport);
|
|
// NEWTON_ASSERT (plane.Evalue(m_boxOrigin) < 0.0f);
|
|
dgPlane &tmpPlane = planesArray[planeCount];
|
|
tmpPlane = plane;
|
|
planesArray[planeCount].m_index = i;
|
|
planesArray[planeCount].m_face = face;
|
|
planeCount++;
|
|
NEWTON_ASSERT(planeCount < dgInt32(sizeof(planesArray) / sizeof(planesArray[0])));
|
|
}
|
|
}
|
|
}
|
|
|
|
m_boundPlanesCount = 0;
|
|
for (dgInt32 i = 0; i < planeCount; i++) {
|
|
dgPlaneLocation &plane = planesArray[i];
|
|
if (plane.m_face == m_faceArray[plane.m_index]) {
|
|
Swap(m_faceArray[plane.m_index], m_faceArray[m_boundPlanesCount]);
|
|
} else {
|
|
dgInt32 j;
|
|
for (j = m_boundPlanesCount; j < m_faceCount; j++) {
|
|
if (plane.m_face == m_faceArray[j]) {
|
|
Swap(m_faceArray[j], m_faceArray[m_boundPlanesCount]);
|
|
break;
|
|
}
|
|
}
|
|
NEWTON_ASSERT(j < m_faceCount);
|
|
}
|
|
m_boundPlanesCount++;
|
|
}
|
|
|
|
m_destructionImpulse = dgFloat32(1.0e20f);
|
|
}
|
|
|
|
dgCollisionConvexHull::dgCollisionConvexHull(dgWorld *const world,
|
|
dgDeserialize deserialization, void *const userData) : dgCollisionConvex(world, deserialization, userData) {
|
|
m_rtti |= dgCollisionConvexHull_RTTI;
|
|
deserialization(userData, &m_vertexCount, sizeof(dgInt32));
|
|
deserialization(userData, &m_vertexCount, sizeof(dgInt32));
|
|
deserialization(userData, &m_faceCount, sizeof(dgInt32));
|
|
deserialization(userData, &m_edgeCount, sizeof(dgInt32));
|
|
deserialization(userData, &m_boundPlanesCount, sizeof(dgInt32));
|
|
deserialization(userData, &m_destructionImpulse, sizeof(dgFloat32));
|
|
|
|
m_vertex = (dgVector *)m_allocator->Malloc(
|
|
dgInt32(m_vertexCount * sizeof(dgVector)));
|
|
m_simplex = (dgConvexSimplexEdge *)m_allocator->Malloc(
|
|
dgInt32(m_edgeCount * sizeof(dgConvexSimplexEdge)));
|
|
m_faceArray = (dgConvexSimplexEdge **)m_allocator->Malloc(
|
|
dgInt32(m_faceCount * sizeof(dgConvexSimplexEdge *)));
|
|
|
|
deserialization(userData, m_vertex, m_vertexCount * sizeof(dgVector));
|
|
|
|
for (dgInt32 i = 0; i < m_edgeCount; i++) {
|
|
dgInt32 serialization[4];
|
|
deserialization(userData, serialization, sizeof(serialization));
|
|
|
|
m_simplex[i].m_vertex = serialization[0];
|
|
m_simplex[i].m_twin = m_simplex + serialization[1];
|
|
m_simplex[i].m_next = m_simplex + serialization[2];
|
|
m_simplex[i].m_prev = m_simplex + serialization[3];
|
|
}
|
|
|
|
for (dgInt32 i = 0; i < m_faceCount; i++) {
|
|
dgInt32 faceOffset;
|
|
deserialization(userData, &faceOffset, sizeof(dgInt32));
|
|
m_faceArray[i] = m_simplex + faceOffset;
|
|
}
|
|
|
|
SetVolumeAndCG();
|
|
}
|
|
|
|
dgCollisionConvexHull::~dgCollisionConvexHull() {
|
|
if (m_faceArray) {
|
|
m_allocator->Free(m_faceArray);
|
|
}
|
|
}
|
|
|
|
dgInt32 dgCollisionConvexHull::GetFaceIndices(dgInt32 index,
|
|
dgInt32 *indices) const {
|
|
dgInt32 count;
|
|
|
|
count = 0;
|
|
const dgConvexSimplexEdge *face = m_faceArray[index];
|
|
do {
|
|
indices[count] = face->m_vertex;
|
|
count++;
|
|
face = face->m_next;
|
|
} while (face != m_faceArray[index]);
|
|
|
|
return count;
|
|
}
|
|
|
|
dgBigVector dgCollisionConvexHull::FaceNormal(const dgEdge *face,
|
|
const dgBigVector *const pool) const {
|
|
const dgEdge *edge = face;
|
|
dgBigVector p0(pool[edge->m_incidentVertex]);
|
|
edge = edge->m_next;
|
|
|
|
dgBigVector p1(pool[edge->m_incidentVertex]);
|
|
dgBigVector e1(p1 - p0);
|
|
|
|
dgBigVector normal(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
|
|
dgFloat32(0.0f));
|
|
for (edge = edge->m_next; edge != face; edge = edge->m_next) {
|
|
dgBigVector p2(pool[edge->m_incidentVertex]);
|
|
dgBigVector e2(p2 - p0);
|
|
dgBigVector n1(e1 * e2);
|
|
#ifdef _DEBUG
|
|
dgFloat64 mag = normal % n1;
|
|
NEWTON_ASSERT(mag >= -dgFloat32(0.1f));
|
|
#endif
|
|
normal += n1;
|
|
e1 = e2;
|
|
}
|
|
dgFloat64 den = sqrt(normal % normal) + dgFloat64(1.0e-24f);
|
|
normal = normal.Scale(dgFloat64(1.0f) / den);
|
|
|
|
#ifdef _DEBUG
|
|
edge = face;
|
|
dgBigVector e0(
|
|
pool[edge->m_incidentVertex] - pool[edge->m_prev->m_incidentVertex]);
|
|
do {
|
|
dgBigVector de1(
|
|
pool[edge->m_next->m_incidentVertex] - pool[edge->m_incidentVertex]);
|
|
dgBigVector n1(e0 * de1);
|
|
dgFloat64 x = normal % n1;
|
|
NEWTON_ASSERT(x > -dgFloat64(0.01f));
|
|
e0 = de1;
|
|
edge = edge->m_next;
|
|
} while (edge != face);
|
|
#endif
|
|
|
|
return normal;
|
|
}
|
|
|
|
bool dgCollisionConvexHull::RemoveCoplanarEdge(dgPolyhedra &polyhedra,
|
|
const dgBigVector *const hullVertexArray) const {
|
|
bool removeEdge = false;
|
|
// remove coplanar edges
|
|
dgInt32 mark = polyhedra.IncLRU();
|
|
dgPolyhedra::Iterator iter(polyhedra);
|
|
for (iter.Begin(); iter;) {
|
|
dgEdge *edge0 = &(*iter);
|
|
iter++;
|
|
|
|
if (edge0->m_incidentFace != -1) {
|
|
|
|
if (edge0->m_mark < mark) {
|
|
edge0->m_mark = mark;
|
|
edge0->m_twin->m_mark = mark;
|
|
dgBigVector normal0(FaceNormal(edge0, &hullVertexArray[0]));
|
|
dgBigVector normal1(FaceNormal(edge0->m_twin, &hullVertexArray[0]));
|
|
|
|
dgFloat64 test = normal0 % normal1;
|
|
if (test > dgFloat64(0.99995f)) {
|
|
|
|
if ((edge0->m_twin->m_next->m_twin->m_next != edge0) && (edge0->m_next->m_twin->m_next != edge0->m_twin)) {
|
|
#define DG_MAX_EDGE_ANGLE dgFloat32(1.0e-3f)
|
|
|
|
if (edge0->m_twin == &(*iter)) {
|
|
if (iter) {
|
|
iter++;
|
|
}
|
|
}
|
|
|
|
dgBigVector e1(
|
|
hullVertexArray[edge0->m_twin->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_incidentVertex]);
|
|
dgBigVector e0(
|
|
hullVertexArray[edge0->m_incidentVertex] - hullVertexArray[edge0->m_prev->m_incidentVertex]);
|
|
|
|
NEWTON_ASSERT((e0 % e0) >= dgFloat64(0.0f));
|
|
NEWTON_ASSERT((e1 % e1) >= dgFloat64(0.0f));
|
|
|
|
e0 = e0.Scale(dgFloat64(1.0f) / sqrt(e0 % e0));
|
|
e1 = e1.Scale(dgFloat64(1.0f) / sqrt(e1 % e1));
|
|
dgBigVector n1(e0 * e1);
|
|
|
|
dgFloat64 projection = n1 % normal0;
|
|
if (projection >= DG_MAX_EDGE_ANGLE) {
|
|
|
|
dgBigVector ee1(
|
|
hullVertexArray[edge0->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_incidentVertex]);
|
|
dgBigVector ee0(
|
|
hullVertexArray[edge0->m_twin->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_prev->m_incidentVertex]);
|
|
NEWTON_ASSERT((ee0 % ee0) >= dgFloat64(0.0f));
|
|
NEWTON_ASSERT((ee1 % ee1) >= dgFloat64(0.0f));
|
|
// ee0 = ee0.Scale (dgRsqrt (ee0 % ee0));
|
|
// ee1 = ee1.Scale (dgRsqrt (ee1 % ee1));
|
|
ee0 = ee0.Scale(dgFloat64(1.0f) / sqrt(ee0 % ee0));
|
|
ee1 = ee1.Scale(dgFloat64(1.0f) / sqrt(ee1 % ee1));
|
|
|
|
dgBigVector nn1(ee0 * ee1);
|
|
projection = nn1 % normal0;
|
|
if (projection >= DG_MAX_EDGE_ANGLE) {
|
|
NEWTON_ASSERT(&(*iter) != edge0);
|
|
NEWTON_ASSERT(&(*iter) != edge0->m_twin);
|
|
polyhedra.DeleteEdge(edge0);
|
|
removeEdge = true;
|
|
}
|
|
}
|
|
|
|
} else {
|
|
dgEdge *next = edge0->m_next;
|
|
dgEdge *prev = edge0->m_prev;
|
|
polyhedra.DeleteEdge(edge0);
|
|
for (edge0 = next; edge0->m_prev->m_twin == edge0; edge0 = next) {
|
|
next = edge0->m_next;
|
|
polyhedra.DeleteEdge(edge0);
|
|
}
|
|
|
|
for (edge0 = prev; edge0->m_next->m_twin == edge0; edge0 = prev) {
|
|
prev = edge0->m_prev;
|
|
polyhedra.DeleteEdge(edge0);
|
|
}
|
|
iter.Begin();
|
|
removeEdge = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return removeEdge;
|
|
}
|
|
|
|
bool dgCollisionConvexHull::CheckConvex(dgPolyhedra &polyhedra1,
|
|
const dgBigVector *hullVertexArray) const {
|
|
dgPolyhedra polyhedra(polyhedra1);
|
|
|
|
dgPolyhedra::Iterator iter(polyhedra);
|
|
dgBigVector center(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
|
|
dgFloat32(0.0f));
|
|
|
|
dgInt32 count = 0;
|
|
dgInt32 mark = polyhedra.IncLRU();
|
|
for (iter.Begin(); iter; iter++) {
|
|
dgEdge *const edge = &(*iter);
|
|
if (edge->m_mark < mark) {
|
|
count++;
|
|
center += hullVertexArray[edge->m_incidentVertex];
|
|
dgEdge *ptr = edge;
|
|
do {
|
|
ptr->m_mark = mark;
|
|
ptr = ptr->m_twin->m_next;
|
|
} while (ptr != edge);
|
|
}
|
|
}
|
|
center = center.Scale(dgFloat64(1.0f) / dgFloat64(count));
|
|
|
|
for (iter.Begin(); iter; iter++) {
|
|
dgEdge *const edge = &(*iter);
|
|
dgBigVector normal0(FaceNormal(edge, hullVertexArray));
|
|
dgBigVector normal1(FaceNormal(edge->m_twin, hullVertexArray));
|
|
|
|
dgBigPlane plane0(normal0,
|
|
-(normal0 % hullVertexArray[edge->m_incidentVertex]));
|
|
dgBigPlane plane1(normal1,
|
|
-(normal1 % hullVertexArray[edge->m_twin->m_incidentVertex]));
|
|
dgFloat64 test0 = plane0.Evalue(center);
|
|
if (test0 > dgFloat64(1.0e-3f)) {
|
|
return false;
|
|
}
|
|
dgFloat64 test1 = plane1.Evalue(center);
|
|
// if (test1 > dgFloat64 (0.0f)) {
|
|
if (test1 > dgFloat64(1.0e-3f)) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool dgCollisionConvexHull::Create(dgInt32 count, dgInt32 strideInBytes,
|
|
const dgFloat32 *const vertexArray, dgFloat32 tolerance) {
|
|
dgInt32 stride = strideInBytes / sizeof(dgFloat32);
|
|
dgStack<dgFloat64> buffer(3 * count);
|
|
for (dgInt32 i = 0; i < count; i++) {
|
|
buffer[i * 3 + 0] = vertexArray[i * stride + 0];
|
|
buffer[i * 3 + 1] = vertexArray[i * stride + 1];
|
|
buffer[i * 3 + 2] = vertexArray[i * stride + 2];
|
|
}
|
|
|
|
dgConvexHull3d *convexHull = new (GetAllocator()) dgConvexHull3d(
|
|
GetAllocator(), &buffer[0], 3 * sizeof(dgFloat64), count, tolerance);
|
|
if (!convexHull->GetCount()) {
|
|
delete convexHull;
|
|
return false;
|
|
}
|
|
|
|
// check for degenerated faces
|
|
for (bool success = false; !success;) {
|
|
success = true;
|
|
const dgBigVector *const hullVertexArray = convexHull->GetVertexPool();
|
|
|
|
dgStack<dgInt8> mask(convexHull->GetVertexCount());
|
|
memset(&mask[0], 1, mask.GetSizeInBytes());
|
|
for (dgConvexHull3d::dgListNode *node = convexHull->GetFirst(); node; node =
|
|
node->GetNext()) {
|
|
dgConvexHull3DFace &face = node->GetInfo();
|
|
const dgBigVector &p0 = hullVertexArray[face.m_index[0]];
|
|
const dgBigVector &p1 = hullVertexArray[face.m_index[1]];
|
|
const dgBigVector &p2 = hullVertexArray[face.m_index[2]];
|
|
dgBigVector p1p0(p1 - p0);
|
|
dgBigVector p2p0(p2 - p0);
|
|
dgBigVector normal(p2p0 * p1p0);
|
|
dgFloat64 mag2 = normal % normal;
|
|
if (mag2 < dgFloat64(1.0e-6f * 1.0e-6f)) {
|
|
success = false;
|
|
dgInt32 index = -1;
|
|
dgBigVector p2p1(p2 - p1);
|
|
dgFloat64 dist10 = p1p0 % p1p0;
|
|
dgFloat64 dist20 = p2p0 % p2p0;
|
|
dgFloat64 dist21 = p2p1 % p2p1;
|
|
if ((dist10 >= dist20) && (dist10 >= dist21)) {
|
|
index = 2;
|
|
} else if ((dist20 >= dist10) && (dist20 >= dist21)) {
|
|
index = 1;
|
|
} else if ((dist21 >= dist10) && (dist21 >= dist20)) {
|
|
index = 0;
|
|
}
|
|
NEWTON_ASSERT(index != -1);
|
|
mask[face.m_index[index]] = 0;
|
|
}
|
|
}
|
|
if (!success) {
|
|
dgInt32 cnt = 0;
|
|
dgInt32 vertexCount = convexHull->GetVertexCount();
|
|
for (dgInt32 i = 0; i < vertexCount; i++) {
|
|
if (mask[i]) {
|
|
buffer[cnt * 3 + 0] = hullVertexArray[i].m_x;
|
|
buffer[cnt * 3 + 1] = hullVertexArray[i].m_y;
|
|
buffer[cnt * 3 + 2] = hullVertexArray[i].m_z;
|
|
cnt++;
|
|
}
|
|
}
|
|
delete convexHull;
|
|
convexHull = new (GetAllocator()) dgConvexHull3d(GetAllocator(),
|
|
&buffer[0], 3 * sizeof(dgFloat64), cnt, tolerance);
|
|
}
|
|
}
|
|
|
|
dgInt32 vertexCount = convexHull->GetVertexCount();
|
|
const dgBigVector *const hullVertexArray = convexHull->GetVertexPool();
|
|
|
|
dgPolyhedra polyhedra(GetAllocator());
|
|
polyhedra.BeginFace();
|
|
for (dgConvexHull3d::dgListNode *node = convexHull->GetFirst(); node; node =
|
|
node->GetNext()) {
|
|
dgConvexHull3DFace &face = node->GetInfo();
|
|
polyhedra.AddFace(face.m_index[0], face.m_index[1], face.m_index[2]);
|
|
}
|
|
|
|
polyhedra.EndFace();
|
|
|
|
if (vertexCount > 4) {
|
|
bool edgeRemoved = false;
|
|
while (RemoveCoplanarEdge(polyhedra, hullVertexArray)) {
|
|
edgeRemoved = true;
|
|
}
|
|
if (edgeRemoved) {
|
|
if (!CheckConvex(polyhedra, hullVertexArray)) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
dgInt32 maxEdgeCount = polyhedra.GetCount();
|
|
|
|
dgStack<dgEdge *> stack(1024 + maxEdgeCount);
|
|
dgEdge *firstFace = &polyhedra.GetRoot()->GetInfo();
|
|
|
|
NEWTON_ASSERT(firstFace->m_twin->m_next != firstFace);
|
|
|
|
dgInt32 stackIndex = 1;
|
|
stack[0] = firstFace;
|
|
|
|
dgStack<dgInt32> vertexMap(vertexCount);
|
|
memset(&vertexMap[0], -1, vertexCount * sizeof(dgInt32));
|
|
|
|
// m_edgeCount = 0;
|
|
// m_vertexCount = 0;
|
|
|
|
dgInt32 i1 = polyhedra.IncLRU();
|
|
while (stackIndex) {
|
|
stackIndex--;
|
|
dgEdge *const edge0 = stack[stackIndex];
|
|
|
|
if (edge0->m_mark != i1) {
|
|
if (vertexMap[edge0->m_incidentVertex] == -1) {
|
|
vertexMap[edge0->m_incidentVertex] = m_vertexCount;
|
|
m_vertexCount++;
|
|
}
|
|
dgEdge *ptr = edge0;
|
|
do {
|
|
stack[stackIndex] = ptr->m_twin;
|
|
stackIndex++;
|
|
ptr->m_mark = i1;
|
|
ptr->m_userData = m_edgeCount;
|
|
m_edgeCount++;
|
|
ptr = ptr->m_twin->m_next;
|
|
} while (ptr != edge0);
|
|
}
|
|
}
|
|
|
|
m_vertex = (dgVector *)m_allocator->Malloc(
|
|
dgInt32(m_vertexCount * sizeof(dgVector)));
|
|
m_simplex = (dgConvexSimplexEdge *)m_allocator->Malloc(
|
|
dgInt32(m_edgeCount * sizeof(dgConvexSimplexEdge)));
|
|
|
|
for (dgInt32 i = 0; i < vertexCount; i++) {
|
|
if (vertexMap[i] != -1) {
|
|
m_vertex[vertexMap[i]] = hullVertexArray[i];
|
|
m_vertex[vertexMap[i]].m_w = dgFloat32(1.0f);
|
|
}
|
|
}
|
|
|
|
i1 = polyhedra.IncLRU();
|
|
stackIndex = 1;
|
|
stack[0] = firstFace;
|
|
while (stackIndex) {
|
|
|
|
stackIndex--;
|
|
dgEdge *const edge0 = stack[stackIndex];
|
|
|
|
if (edge0->m_mark != i1) {
|
|
|
|
dgEdge *ptr = edge0;
|
|
do {
|
|
ptr->m_mark = i1;
|
|
stack[stackIndex] = ptr->m_twin;
|
|
stackIndex++;
|
|
|
|
dgConvexSimplexEdge *const simplexPtr = &m_simplex[ptr->m_userData];
|
|
simplexPtr->m_vertex = vertexMap[ptr->m_incidentVertex];
|
|
simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
|
|
simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
|
|
simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
|
|
|
|
ptr = ptr->m_twin->m_next;
|
|
} while (ptr != edge0);
|
|
}
|
|
}
|
|
|
|
SetVolumeAndCG();
|
|
m_faceCount = 0;
|
|
dgStack<char> mark(m_edgeCount);
|
|
memset(&mark[0], 0, m_edgeCount * sizeof(dgInt8));
|
|
|
|
dgStack<dgConvexSimplexEdge *> faceArray(m_edgeCount);
|
|
for (dgInt32 i = 0; i < m_edgeCount; i++) {
|
|
dgConvexSimplexEdge *const face = &m_simplex[i];
|
|
if (!mark[i]) {
|
|
dgConvexSimplexEdge *ptr = face;
|
|
do {
|
|
NEWTON_ASSERT((ptr - m_simplex) >= 0);
|
|
mark[dgInt32(ptr - m_simplex)] = '1';
|
|
ptr = ptr->m_next;
|
|
} while (ptr != face);
|
|
|
|
faceArray[m_faceCount] = face;
|
|
m_faceCount++;
|
|
}
|
|
}
|
|
m_faceArray = (dgConvexSimplexEdge **)m_allocator->Malloc(
|
|
dgInt32(m_faceCount * sizeof(dgConvexSimplexEdge *)));
|
|
memcpy(m_faceArray, &faceArray[0],
|
|
m_faceCount * sizeof(dgConvexSimplexEdge *));
|
|
|
|
delete convexHull;
|
|
return true;
|
|
}
|
|
|
|
dgInt32 dgCollisionConvexHull::CalculateSignature() const {
|
|
NEWTON_ASSERT(0);
|
|
return dgInt32(GetSignature());
|
|
}
|
|
|
|
void dgCollisionConvexHull::SetBreakImpulse(dgFloat32 force) {
|
|
m_destructionImpulse = force;
|
|
}
|
|
|
|
dgFloat32 dgCollisionConvexHull::GetBreakImpulse() const {
|
|
return m_destructionImpulse;
|
|
}
|
|
|
|
void dgCollisionConvexHull::SetCollisionBBox(const dgVector &p0__,
|
|
const dgVector &p1__) {
|
|
NEWTON_ASSERT(0);
|
|
}
|
|
|
|
void dgCollisionConvexHull::DebugCollision(const dgMatrix &matrixPtr,
|
|
OnDebugCollisionMeshCallback callback, void *const userData) const {
|
|
dgInt32 i;
|
|
dgInt32 count;
|
|
dgConvexSimplexEdge *ptr;
|
|
dgConvexSimplexEdge *face;
|
|
|
|
dgStack<dgTriplex> tmp(m_vertexCount);
|
|
dgMatrix matrix(GetOffsetMatrix() * matrixPtr);
|
|
matrix.TransformTriplex(&tmp[0].m_x, sizeof(dgTriplex), &m_vertex[0].m_x,
|
|
sizeof(dgVector), m_vertexCount);
|
|
|
|
for (i = 0; i < m_faceCount; i++) {
|
|
face = m_faceArray[i];
|
|
ptr = face;
|
|
count = 0;
|
|
dgTriplex vertex[256];
|
|
do {
|
|
vertex[count] = tmp[ptr->m_vertex];
|
|
count++;
|
|
ptr = ptr->m_next;
|
|
} while (ptr != face);
|
|
callback(userData, count, &vertex[0].m_x, 0);
|
|
}
|
|
}
|
|
|
|
void dgCollisionConvexHull::GetCollisionInfo(dgCollisionInfo *info) const {
|
|
dgCollisionConvex::GetCollisionInfo(info);
|
|
|
|
info->m_offsetMatrix = GetOffsetMatrix();
|
|
// strcpy (info->m_collisionType, "convexHull");
|
|
info->m_collisionType = m_collsionId;
|
|
|
|
info->m_convexHull.m_vertexCount = m_vertexCount;
|
|
info->m_convexHull.m_strideInBytes = sizeof(dgVector);
|
|
info->m_convexHull.m_faceCount = m_faceCount;
|
|
info->m_convexHull.m_vertex = &m_vertex[0];
|
|
}
|
|
|
|
void dgCollisionConvexHull::Serialize(dgSerialize callback,
|
|
void *const userData) const {
|
|
SerializeLow(callback, userData);
|
|
|
|
callback(userData, &m_vertexCount, sizeof(dgInt32));
|
|
callback(userData, &m_vertexCount, sizeof(dgInt32));
|
|
callback(userData, &m_faceCount, sizeof(dgInt32));
|
|
callback(userData, &m_edgeCount, sizeof(dgInt32));
|
|
callback(userData, &m_boundPlanesCount, sizeof(dgInt32));
|
|
callback(userData, &m_destructionImpulse, sizeof(dgFloat32));
|
|
|
|
callback(userData, m_vertex, m_vertexCount * sizeof(dgVector));
|
|
|
|
for (dgInt32 i = 0; i < m_edgeCount; i++) {
|
|
dgInt32 serialization[4];
|
|
serialization[0] = m_simplex[i].m_vertex;
|
|
serialization[1] = dgInt32(m_simplex[i].m_twin - m_simplex);
|
|
serialization[2] = dgInt32(m_simplex[i].m_next - m_simplex);
|
|
serialization[3] = dgInt32(m_simplex[i].m_prev - m_simplex);
|
|
callback(userData, serialization, sizeof(serialization));
|
|
}
|
|
|
|
for (dgInt32 i = 0; i < m_faceCount; i++) {
|
|
dgInt32 faceOffset;
|
|
faceOffset = dgInt32(m_faceArray[i] - m_simplex);
|
|
callback(userData, &faceOffset, sizeof(dgInt32));
|
|
}
|
|
}
|
|
|
|
bool dgCollisionConvexHull::OOBBTest(const dgMatrix &matrix,
|
|
const dgCollisionConvex *const shape, void *const cacheOrder) const {
|
|
bool ret;
|
|
NEWTON_ASSERT(cacheOrder);
|
|
|
|
ret = dgCollisionConvex::OOBBTest(matrix, shape, cacheOrder);
|
|
if (ret) {
|
|
const dgConvexSimplexEdge *const *faceArray = m_faceArray;
|
|
dgCollisionBoundPlaneCache *const cache =
|
|
(dgCollisionBoundPlaneCache *)cacheOrder;
|
|
|
|
for (dgInt32 i = 0; i < dgInt32(sizeof(cache->m_planes) / sizeof(dgPlane));
|
|
i++) {
|
|
dgFloat32 dist;
|
|
const dgPlane &plane = cache->m_planes[i];
|
|
if ((plane % plane) > dgFloat32(0.0f)) {
|
|
dgVector dir(matrix.UnrotateVector(plane.Scale(-1.0f)));
|
|
dir.m_w = dgFloat32(0.0f);
|
|
dgVector p(matrix.TransformVector(shape->SupportVertex(dir)));
|
|
dist = plane.Evalue(p);
|
|
if (dist > dgFloat32(0.1f)) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
for (dgInt32 i = 0; i < m_boundPlanesCount; i++) {
|
|
dgInt32 i0;
|
|
dgInt32 i1;
|
|
dgInt32 i2;
|
|
dgFloat32 dist;
|
|
|
|
const dgConvexSimplexEdge *const face = faceArray[i];
|
|
i0 = face->m_prev->m_vertex;
|
|
i1 = face->m_vertex;
|
|
i2 = face->m_next->m_vertex;
|
|
const dgVector &p0 = m_vertex[i0];
|
|
|
|
dgVector normal((m_vertex[i1] - p0) * (m_vertex[i2] - p0));
|
|
normal = normal.Scale(dgFloat32(1.0f) / dgSqrt(normal % normal));
|
|
|
|
dgVector dir(matrix.UnrotateVector(normal.Scale(-1.0f)));
|
|
dir.m_w = dgFloat32(0.0f);
|
|
dgVector p(matrix.TransformVector(shape->SupportVertex(dir)));
|
|
|
|
//NEWTON_ASSERT ((normal % (m_boxOrigin - p0)) < 0.0f);
|
|
dist = normal % (p - p0);
|
|
if (dist > dgFloat32(0.1f)) {
|
|
for (dgInt32 j = 0;
|
|
j < (dgInt32(sizeof(cache->m_planes) / sizeof(dgPlane)) - 1); j++) {
|
|
cache->m_planes[j + 1] = cache->m_planes[j];
|
|
}
|
|
cache->m_planes[1] = dgPlane(normal, -(normal % p0));
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return ret;
|
|
}
|