madcow/scummvm-cursorfix
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity
Files
main
scummvm-cursorfix/engines/hpl1/engine/libraries/newton/physics/dgCollisionMesh.cpp
Leon Krieg 5b11698731 Initial commit
2026-02-02 04:50:13 +01:00

1482 lines
49 KiB
C++
Raw Permalink Blame History

/* 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 "dgCollisionMesh.h"
#include "dgBody.h"
#include "dgCollisionSphere.h"
#include "dgContact.h"
#include "dgWorld.h"
#include "hpl1/engine/libraries/newton/core/dg.h"
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
#define DG_CONVEX_POLYGON_CRC 0x12341234
dgCollisionMesh::dgCollisionConvexPolygon::dgCollisionConvexPolygon(dgMemoryAllocator *const allocator)
: dgCollisionConvex(allocator, DG_CONVEX_POLYGON_CRC, dgGetIdentityMatrix(), m_polygonCollision) {
m_count = 0;
m_index = 0;
m_vertex = NULL;
m_stride = 0;
m_paddedCount = 0;
m_isEdgeIntersection = false;
m_rtti |= dgCollisionConvexPolygon_RTTI;
for (unsigned i = 0; i < (sizeof(m_localPoly) / sizeof(m_localPoly[0])); i++)
m_localPoly[i] = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
m_normal = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
// m_aabbP0 = dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
// m_aabbP1 = dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
dgCollisionMesh::dgCollisionConvexPolygon::~dgCollisionConvexPolygon() {
}
dgInt32 dgCollisionMesh::dgCollisionConvexPolygon::CalculateSignature() const {
return DG_CONVEX_POLYGON_CRC;
}
void dgCollisionMesh::dgCollisionConvexPolygon::SetCollisionBBox(const dgVector &p0__, const dgVector &p1__) {
NEWTON_ASSERT(0);
}
void dgCollisionMesh::dgCollisionConvexPolygon::Serialize(dgSerialize callback, void *const userData) const {
NEWTON_ASSERT(0);
}
dgFloat32 dgCollisionMesh::dgCollisionConvexPolygon::RayCast(
const dgVector &localP0,
const dgVector &localP1,
dgContactPoint &contactOut,
OnRayPrecastAction preFilter,
const dgBody *const body,
void *userData) const {
NEWTON_ASSERT(0);
return dgFloat32(0.0f);
}
dgFloat32 dgCollisionMesh::dgCollisionConvexPolygon::GetVolume() const {
NEWTON_ASSERT(0);
return dgFloat32(0.0f);
}
dgFloat32 dgCollisionMesh::dgCollisionConvexPolygon::GetBoxMinRadius() const {
NEWTON_ASSERT(0);
return dgFloat32(0.0f);
}
dgFloat32 dgCollisionMesh::dgCollisionConvexPolygon::GetBoxMaxRadius() const {
NEWTON_ASSERT(0);
return dgFloat32(0.0f);
}
bool dgCollisionMesh::dgCollisionConvexPolygon::OOBBTest(const dgMatrix &matrix, const dgCollisionConvex *const shape, void *chache) const {
NEWTON_ASSERT(0);
return true;
}
void dgCollisionMesh::dgCollisionConvexPolygon::CalculateInertia(dgVector &inertia, dgVector &origin) const {
inertia.m_x = dgFloat32(0.0f);
inertia.m_y = dgFloat32(0.0f);
inertia.m_z = dgFloat32(0.0f);
origin.m_x = dgFloat32(0.0f);
origin.m_y = dgFloat32(0.0f);
origin.m_z = dgFloat32(0.0f);
}
dgVector dgCollisionMesh::dgCollisionConvexPolygon::SupportVertex(const dgVector &dir) const {
NEWTON_ASSERT(dgAbsf(dir % dir - 1.0f) < dgFloat32(1.0e-2f));
dgInt32 index = 0;
dgFloat32 val = m_localPoly[0] % dir;
for (dgInt32 i = 1; i < m_count; i++) {
dgFloat32 val1 = m_localPoly[i] % dir;
if (val1 > val) {
val = val1;
index = i;
}
}
return m_localPoly[index];
}
dgVector dgCollisionMesh::dgCollisionConvexPolygon::SupportVertexSimd(const dgVector &dir) const {
#ifdef DG_BUILD_SIMD_CODE
NEWTON_ASSERT(dgAbsf(dir % dir - 1.0f) < dgFloat32(1.0e-3f));
simd_type dirX = simd_permut_v(*(simd_type *)&dir, *(simd_type *)&dir, PURMUT_MASK(0, 0, 0, 0));
simd_type dirY = simd_permut_v(*(simd_type *)&dir, *(simd_type *)&dir, PURMUT_MASK(1, 1, 1, 1));
simd_type dirZ = simd_permut_v(*(simd_type *)&dir, *(simd_type *)&dir, PURMUT_MASK(2, 2, 2, 2));
simd_type dot = simd_mul_add_v(simd_mul_add_v(simd_mul_v(dirX, *(simd_type *)&m_localPolySimd[0]),
dirY, *(simd_type *)&m_localPolySimd[1]),
dirZ, *(simd_type *)&m_localPolySimd[2]);
simd_type index = *(simd_type *)&m_index_0123;
simd_type indexAcc = index;
for (dgInt32 i = 3; i < m_paddedCount; i += 3) {
indexAcc = simd_add_v(indexAcc, *(simd_type *)&m_indexStep);
simd_type dot1 = simd_mul_add_v(simd_mul_add_v(simd_mul_v(dirX, *(simd_type *)&m_localPolySimd[i + 0]),
dirY, *(simd_type *)&m_localPolySimd[i + 1]),
dirZ, *(simd_type *)&m_localPolySimd[i + 2]);
simd_type mask = simd_cmpgt_v(dot1, dot);
dot = simd_max_v(dot1, dot);
index = simd_or_v(simd_and_v(indexAcc, mask), simd_andnot_v(index, mask));
}
dirX = simd_permut_v(dot, dot, PURMUT_MASK(0, 0, 3, 2));
simd_type mask = simd_cmpge_v(dot, dirX);
dot = simd_max_v(dot, dirX);
index = simd_or_v(simd_and_v(index, mask), simd_andnot_v(simd_permut_v(index, index, PURMUT_MASK(0, 0, 3, 2)), mask));
mask = simd_cmpge_s(dot, simd_permut_v(dot, dot, PURMUT_MASK(0, 0, 0, 1)));
dgInt32 i = simd_store_is(simd_or_v(simd_and_v(index, mask), simd_andnot_v(simd_permut_v(index, index, PURMUT_MASK(0, 0, 0, 1)), mask)));
// dgFloat32 fIndex;
// simd_store_s (simd_or_v (simd_and_v(index, mask), simd_andnot_v (simd_permut_v (index, index, PURMUT_MASK(0, 0, 0, 1)), mask)), &fIndex);
// dgInt32 i = dgFastInt (fIndex);
return m_localPoly[i];
#else
return dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
#endif
}
void dgCollisionMesh::dgCollisionConvexPolygon::CalculateNormalSimd() {
// CalculateNormal();
#ifdef DG_BUILD_SIMD_CODE
if (m_normalIndex) {
m_normal = dgVector(&m_vertex[m_normalIndex * m_stride]);
} else {
simd_type e10;
simd_type e21;
simd_type tmp0;
simd_type mag2;
simd_type normal;
e10 = simd_sub_v(*(simd_type *)&m_localPoly[1], *(simd_type *)&m_localPoly[0]);
e21 = simd_sub_v(*(simd_type *)&m_localPoly[2], *(simd_type *)&m_localPoly[1]);
normal = simd_mul_sub_v(simd_mul_v(simd_permut_v(e10, e10, PURMUT_MASK(3, 0, 2, 1)), simd_permut_v(e21, e21, PURMUT_MASK(3, 1, 0, 2))),
simd_permut_v(e10, e10, PURMUT_MASK(3, 1, 0, 2)), simd_permut_v(e21, e21, PURMUT_MASK(3, 0, 2, 1)));
NEWTON_ASSERT(((dgFloat32 *)&normal)[3] == dgFloat32(0.0f));
mag2 = simd_mul_v(normal, normal);
mag2 = simd_add_v(mag2, simd_move_hl_v(mag2, mag2));
mag2 = simd_sub_s(simd_add_s(mag2, simd_permut_v(mag2, mag2, PURMUT_MASK(3, 3, 3, 1))), *(simd_type *)&m_negativeTiny);
tmp0 = simd_rsqrt_s(mag2);
mag2 = simd_mul_s(simd_mul_s(*(simd_type *)&m_nrh0p5, tmp0), simd_mul_sub_s(*(simd_type *)&m_nrh3p0, simd_mul_s(mag2, tmp0), tmp0));
(*(simd_type *)&m_normal) = simd_mul_v(normal, simd_permut_v(mag2, mag2, PURMUT_MASK(3, 0, 0, 0)));
}
#endif
}
void dgCollisionMesh::dgCollisionConvexPolygon::CalculateNormal() {
if (m_normalIndex) {
m_normal = dgVector(&m_vertex[m_normalIndex * m_stride]);
} else {
dgVector e10(m_localPoly[1] - m_localPoly[0]);
dgVector e21(m_localPoly[2] - m_localPoly[1]);
dgVector normal(e10 * e21);
NEWTON_ASSERT((normal % normal) > dgFloat32(0.0f));
m_normal = normal.Scale(dgRsqrt(normal % normal + dgFloat32(1.0e-24f)));
}
}
dgInt32 dgCollisionMesh::dgCollisionConvexPolygon::QuickTestContinueSimd(const dgCollisionConvex *const hull, const dgMatrix &matrix) {
#ifdef DG_BUILD_SIMD_CODE
dgInt32 ret;
dgFloat32 val1;
m_localPoly[0] = dgVector(&m_vertex[m_index[0] * m_stride]);
m_localPoly[1] = dgVector(&m_vertex[m_index[1] * m_stride]);
m_localPoly[2] = dgVector(&m_vertex[m_index[2] * m_stride]);
CalculateNormalSimd();
dgVector p1(matrix.UntransformVector(hull->SupportVertexSimd(matrix.RotateVectorSimd(m_normal))));
val1 = (p1 - m_localPoly[0]) % m_normal;
ret = (val1 >= dgFloat32(0.0f));
if (ret) {
dgInt32 i1;
dgInt32 i0;
for (i1 = 3; i1 < m_count; i1++) {
m_localPoly[i1] = dgVector(&m_vertex[m_index[i1] * m_stride]);
}
i0 = (m_count + 3) & -4;
for (; i1 < i0; i1++) {
m_localPoly[i1] = m_localPoly[0];
}
i1 = 0;
for (dgInt32 i = 0; i < i0; i += 4) {
m_localPolySimd[i1 + 0] = dgVector(m_localPoly[i + 0].m_x, m_localPoly[i + 1].m_x, m_localPoly[i + 2].m_x, m_localPoly[i + 3].m_x);
m_localPolySimd[i1 + 1] = dgVector(m_localPoly[i + 0].m_y, m_localPoly[i + 1].m_y, m_localPoly[i + 2].m_y, m_localPoly[i + 3].m_y);
m_localPolySimd[i1 + 2] = dgVector(m_localPoly[i + 0].m_z, m_localPoly[i + 1].m_z, m_localPoly[i + 2].m_z, m_localPoly[i + 3].m_z);
i1 += 3;
}
m_paddedCount = i1;
}
return ret;
#else
return 0;
#endif
}
dgInt32 dgCollisionMesh::dgCollisionConvexPolygon::QuickTestContinue(const dgCollisionConvex *hull, const dgMatrix &matrix) {
dgInt32 ret;
dgFloat32 val1;
NEWTON_ASSERT(m_count < dgInt32(sizeof(m_localPoly) / sizeof(m_localPoly[0])));
m_localPoly[0] = dgVector(&m_vertex[m_index[0] * m_stride]);
m_localPoly[1] = dgVector(&m_vertex[m_index[1] * m_stride]);
m_localPoly[2] = dgVector(&m_vertex[m_index[2] * m_stride]);
CalculateNormal();
dgVector p1(matrix.UntransformVector(hull->SupportVertex(matrix.RotateVector(m_normal))));
val1 = (p1 - m_localPoly[0]) % m_normal;
ret = (val1 >= dgFloat32(0.0f));
if (ret) {
for (dgInt32 i = 3; i < m_count; i++) {
m_localPoly[i] = dgVector(&m_vertex[m_index[i] * m_stride]);
}
}
return ret;
}
dgInt32 dgCollisionMesh::dgCollisionConvexPolygon::QuickTestSimd(const dgCollisionConvex *const hull, const dgMatrix &matrix) {
#ifdef DG_BUILD_SIMD_CODE
dgInt32 i;
// dgInt32 i0;
// dgInt32 i1;
dgFloat32 val0;
dgFloat32 val1;
simd_type normal;
simd_type normal1;
dgVector rotatedNormal;
NEWTON_ASSERT(m_count < dgInt32(sizeof(m_localPoly) / sizeof(m_localPoly[0])));
m_localPoly[0] = dgVector(&m_vertex[m_index[0] * m_stride]);
m_localPoly[1] = dgVector(&m_vertex[m_index[1] * m_stride]);
m_localPoly[2] = dgVector(&m_vertex[m_index[2] * m_stride]);
CalculateNormalSimd();
// rotatedNormal = matrix.RotateVector (normal__);
normal = simd_mul_v(*(simd_type *)&m_normal, *(simd_type *)&m_negOne);
normal1 = simd_mul_add_v(simd_mul_add_v(simd_mul_v(*(simd_type *)&matrix[0], simd_permut_v(normal, normal, PURMUT_MASK(3, 0, 0, 0))),
*(simd_type *)&matrix[1], simd_permut_v(normal, normal, PURMUT_MASK(3, 1, 1, 1))),
*(simd_type *)&matrix[2], simd_permut_v(normal, normal, PURMUT_MASK(3, 2, 2, 2)));
(*(simd_type *)&rotatedNormal) = normal1;
dgVector p0(matrix.UntransformVector(hull->SupportVertexSimd(rotatedNormal)));
(*(simd_type *)&rotatedNormal) = simd_mul_v(normal1, *(simd_type *)&m_negOne);
dgVector p1(matrix.UntransformVector(hull->SupportVertexSimd(rotatedNormal)));
val0 = (m_localPoly[0] - p0) % m_normal + dgFloat32(1.0e-1f);
val1 = (m_localPoly[0] - p1) % m_normal - dgFloat32(1.0e-1f);
if (val0 * val1 >= dgFloat32(0.0f)) {
return 0;
}
for (i = 3; i < m_count; i++) {
m_localPoly[i] = dgVector(&m_vertex[m_index[i] * m_stride]);
}
dgInt32 i0 = (m_count + 3) & -4;
for (; i < i0; i++) {
m_localPoly[i] = m_localPoly[0];
}
dgInt32 i1 = 0;
for (dgInt32 i = 0; i < i0; i += 4) {
m_localPolySimd[i1 + 0] = dgVector(m_localPoly[i + 0].m_x, m_localPoly[i + 1].m_x, m_localPoly[i + 2].m_x, m_localPoly[i + 3].m_x);
m_localPolySimd[i1 + 1] = dgVector(m_localPoly[i + 0].m_y, m_localPoly[i + 1].m_y, m_localPoly[i + 2].m_y, m_localPoly[i + 3].m_y);
m_localPolySimd[i1 + 2] = dgVector(m_localPoly[i + 0].m_z, m_localPoly[i + 1].m_z, m_localPoly[i + 2].m_z, m_localPoly[i + 3].m_z);
i1 += 3;
}
m_paddedCount = i1;
return 1;
#else
return 0;
#endif
}
dgInt32 dgCollisionMesh::dgCollisionConvexPolygon::QuickTest(const dgCollisionConvex *const hull, const dgMatrix &matrix) {
NEWTON_ASSERT(m_count < dgInt32(sizeof(m_localPoly) / sizeof(m_localPoly[0])));
m_localPoly[0] = dgVector(&m_vertex[m_index[0] * m_stride]);
m_localPoly[1] = dgVector(&m_vertex[m_index[1] * m_stride]);
m_localPoly[2] = dgVector(&m_vertex[m_index[2] * m_stride]);
CalculateNormal();
// dgVector normal (m_normal.Scale (dgFloat32 (-1.0f)));
dgVector rotatedNormal(matrix.RotateVector(m_normal));
dgVector p0(matrix.UntransformVector(hull->SupportVertex(rotatedNormal.Scale(dgFloat32(-1.0f)))));
dgVector p1(matrix.UntransformVector(hull->SupportVertex(rotatedNormal)));
// dgFloat32 val0 = (p0 - m_localPoly[0]) % normal + dgFloat32 (1.0e-1f);
// dgFloat32 val1 = (p1 - m_localPoly[0]) % normal - dgFloat32 (1.0e-1f);
dgFloat32 val0 = (m_localPoly[0] - p0) % m_normal + dgFloat32(1.0e-1f);
dgFloat32 val1 = (m_localPoly[0] - p1) % m_normal - dgFloat32(1.0e-1f);
if (val0 * val1 >= dgFloat32(0.0f)) {
return 0;
}
for (dgInt32 i = 3; i < m_count; i++) {
m_localPoly[i] = dgVector(&m_vertex[m_index[i] * m_stride]);
}
return 1;
}
dgInt32 dgCollisionMesh::dgCollisionConvexPolygon::ClipContacts(dgInt32 count, dgContactPoint *const contactOut, const dgMatrix &globalMatrix) const {
dgVector normal(globalMatrix.RotateVector(m_normal));
if (m_normalIndex) {
for (dgInt32 i = 0; i < count; i++) {
if (contactOut[i].m_isEdgeContact) {
dgFloat32 dist = contactOut[i].m_normal % normal;
contactOut[i].m_isEdgeContact = 0;
if (dist <= dgFloat32(0.9998f)) {
dgVector point(globalMatrix.UntransformVector(contactOut[i].m_point));
dgInt32 j0 = m_count - 1;
dgInt32 closestEdgeIndex = 0;
contactOut[i].m_isEdgeContact = 1;
dgFloat32 closestEdgeDist = dgFloat32(1.0e20f);
for (dgInt32 j1 = 0; j1 < m_count; j1++) {
dgVector edge(m_localPoly[j1] - m_localPoly[j0]);
dgVector dp(point - m_localPoly[j0]);
dgFloat32 dist2 = dgFloat32(1.0e10f);
dgFloat32 num = dp % edge;
dgFloat32 den = edge % edge;
if (num < 0.0f) {
dist2 = dp % dp;
} else if (num > den) {
dgVector pp(point - m_localPoly[j1]);
dist2 = pp % pp;
} else {
dgVector pp(dp - edge.Scale(num / den));
dist2 = pp % pp;
}
// dgVector p (dp - edge.Scale ((dp % edge) / (edge % edge)));
// dgFloat32 dist2 = p % p;
if (dist2 < closestEdgeDist) {
closestEdgeDist = dist2;
closestEdgeIndex = j0;
}
j0 = j1;
}
NEWTON_ASSERT(m_adjacentNormalIndex);
if (m_adjacentNormalIndex[closestEdgeIndex] == -1) {
contactOut[i].m_normal = normal;
} else {
dgVector aNormal(globalMatrix.RotateVector(dgVector(&m_vertex[m_adjacentNormalIndex[closestEdgeIndex] * m_stride])));
NEWTON_ASSERT((normal % normal) > 0.9999f);
NEWTON_ASSERT((aNormal % aNormal) > 0.9999f);
dgFloat32 dot = normal % aNormal;
if ((dot * dot) > dgFloat32(0.999f)) {
normal += aNormal;
contactOut[i].m_normal = normal.Scale(dgRsqrt(normal % normal));
contactOut[i].m_isEdgeContact = 0;
} else {
dgVector planeNormal(normal * aNormal);
dgVector projectNormal(contactOut[i].m_normal - planeNormal.Scale((planeNormal % contactOut[i].m_normal) / (planeNormal % planeNormal)));
dgVector dirPlane(projectNormal * aNormal);
dgFloat32 dir(dirPlane % planeNormal);
if (dir < dgFloat32(0.0f)) {
contactOut[i].m_normal = aNormal;
contactOut[i].m_isEdgeContact = 0;
}
}
}
}
}
}
} else {
for (dgInt32 i = 0; i < count; i++) {
if (contactOut[i].m_isEdgeContact) {
dgFloat32 dist = contactOut[i].m_normal % normal;
contactOut[i].m_isEdgeContact = (dist < dgFloat32(0.999f));
if (dist < dgFloat32(0.1f)) {
contactOut[i] = contactOut[count - 1];
count--;
i--;
}
}
}
}
return count;
}
void dgCollisionMesh::dgCollisionConvexPolygon::BeamClippingSimd(const dgCollisionConvex *const hull, const dgMatrix &matrix, dgFloat32 dist) {
BeamClipping(hull, matrix, dist);
dgInt32 i0 = (m_count + 3) & -4;
for (dgInt32 i = m_count; i < i0; i++) {
m_localPoly[i] = m_localPoly[0];
}
dgInt32 i1 = 0;
for (dgInt32 i = 0; i < i0; i += 4) {
m_localPolySimd[i1 + 0] = dgVector(m_localPoly[i + 0].m_x, m_localPoly[i + 1].m_x, m_localPoly[i + 2].m_x, m_localPoly[i + 3].m_x);
m_localPolySimd[i1 + 1] = dgVector(m_localPoly[i + 0].m_y, m_localPoly[i + 1].m_y, m_localPoly[i + 2].m_y, m_localPoly[i + 3].m_y);
m_localPolySimd[i1 + 2] = dgVector(m_localPoly[i + 0].m_z, m_localPoly[i + 1].m_z, m_localPoly[i + 2].m_z, m_localPoly[i + 3].m_z);
i1 += 3;
}
m_paddedCount = i1;
}
void dgCollisionMesh::dgCollisionConvexPolygon::BeamClipping(const dgCollisionConvex *const hull, const dgMatrix &matrix, dgFloat32 dist) {
dgPlane planes[4];
dgVector points[sizeof(m_localPoly) / sizeof(m_localPoly[0]) + 8];
DG_CLIPPED_FACE_EDGE clippedFace[sizeof(m_localPoly) / sizeof(m_localPoly[0]) + 8];
dgVector origin(matrix.UnrotateVector(matrix.m_posit.Scale(dgFloat32(-1.0f))));
dgVector dir(m_localPoly[1] - m_localPoly[0]);
NEWTON_ASSERT((dir % dir) > dgFloat32(1.0e-8f));
dir = dir.Scale(dgRsqrt(dir % dir));
dgFloat32 test(dir % origin);
planes[0] = dgPlane(dir, dist - test);
planes[2] = dgPlane(dir.Scale(dgFloat32(-1.0f)), dist + test);
dir = m_normal * dir;
test = dir % origin;
planes[1] = dgPlane(dir, dist - test);
planes[3] = dgPlane(dir.Scale(dgFloat32(-1.0f)), dist + test);
for (dgInt32 i = 0; i < m_count; i++) {
dgInt32 j = i << 1;
points[i] = m_localPoly[i];
clippedFace[j + 0].m_twin = &clippedFace[j + 1];
clippedFace[j + 0].m_next = &clippedFace[j + 2];
clippedFace[j + 0].m_incidentVertex = i;
clippedFace[j + 0].m_incidentNormal = m_adjacentNormalIndex ? m_adjacentNormalIndex[i] : -1;
clippedFace[j + 1].m_twin = &clippedFace[j + 0];
clippedFace[j + 1].m_next = &clippedFace[j - 2];
clippedFace[j + 1].m_incidentVertex = i + 1;
clippedFace[j + 0].m_incidentNormal = -1;
}
clippedFace[1].m_next = &clippedFace[m_count * 2 - 2 + 1];
clippedFace[m_count * 2 - 2].m_next = &clippedFace[0];
clippedFace[m_count * 2 - 2 + 1].m_incidentVertex = 0;
dgInt32 edgeCount = m_count * 2;
dgInt32 indexCount = m_count;
DG_CLIPPED_FACE_EDGE *first = &clippedFace[0];
for (dgInt32 i = 0; i < 4; i++) {
const dgPlane &plane = planes[i];
NEWTON_ASSERT(plane.Evalue(origin) > dgFloat32(0.0f));
dgInt32 conectCount = 0;
DG_CLIPPED_FACE_EDGE *connect[2];
DG_CLIPPED_FACE_EDGE *ptr = first;
DG_CLIPPED_FACE_EDGE *newFirst = first;
dgFloat32 test0 = plane.Evalue(points[ptr->m_incidentVertex]);
do {
dgFloat32 test1 = plane.Evalue(points[ptr->m_next->m_incidentVertex]);
if (test0 > dgFloat32(0.0f)) {
if (test1 <= dgFloat32(0.0f)) {
const dgVector &p0 = points[ptr->m_incidentVertex];
const dgVector &p1 = points[ptr->m_next->m_incidentVertex];
dgVector dp(p1 - p0);
points[indexCount] = p0 - dp.Scale(test0 / (dp % plane));
DG_CLIPPED_FACE_EDGE *const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_twin->m_incidentNormal = -1;
newEdge->m_incidentNormal = ptr->m_incidentNormal;
newEdge->m_incidentVertex = indexCount;
newEdge->m_twin->m_incidentVertex = ptr->m_next->m_incidentVertex;
ptr->m_twin->m_incidentVertex = indexCount;
newEdge->m_next = ptr->m_next;
ptr->m_next->m_twin->m_next = newEdge->m_twin;
newEdge->m_twin->m_next = ptr->m_twin;
ptr->m_next = newEdge;
connect[conectCount] = ptr;
conectCount++;
indexCount++;
edgeCount += 2;
ptr = newEdge;
}
} else {
if (test1 > dgFloat32(0.0f)) {
newFirst = ptr->m_next;
const dgVector &p0 = points[ptr->m_incidentVertex];
const dgVector &p1 = points[ptr->m_next->m_incidentVertex];
dgVector dp(p1 - p0);
points[indexCount] = p0 - dp.Scale(test0 / (dp % plane));
DG_CLIPPED_FACE_EDGE *const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_twin->m_incidentNormal = -1;
newEdge->m_incidentNormal = ptr->m_incidentNormal;
newEdge->m_incidentVertex = indexCount;
newEdge->m_twin->m_incidentVertex = ptr->m_next->m_incidentVertex;
ptr->m_twin->m_incidentVertex = indexCount;
newEdge->m_next = ptr->m_next;
ptr->m_next->m_twin->m_next = newEdge->m_twin;
newEdge->m_twin->m_next = ptr->m_twin;
ptr->m_next = newEdge;
connect[conectCount] = ptr;
conectCount++;
indexCount++;
edgeCount += 2;
ptr = newEdge;
}
}
test0 = test1;
ptr = ptr->m_next;
} while (ptr != first);
if (conectCount) {
first = newFirst;
NEWTON_ASSERT(conectCount == 2);
DG_CLIPPED_FACE_EDGE *const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_incidentNormal = -1;
newEdge->m_twin->m_incidentNormal = -1;
newEdge->m_incidentVertex = connect[0]->m_next->m_incidentVertex;
newEdge->m_twin->m_next = connect[0]->m_next;
connect[0]->m_next = newEdge;
newEdge->m_twin->m_incidentVertex = connect[1]->m_next->m_incidentVertex;
newEdge->m_next = connect[1]->m_next;
connect[1]->m_next = newEdge->m_twin;
edgeCount += 2;
}
}
dgInt32 count = 0;
DG_CLIPPED_FACE_EDGE *ptr = first;
if (m_adjacentNormalIndex) {
m_adjacentNormalIndex = &m_clippEdgeNormal[0];
do {
NEWTON_ASSERT(ptr->m_incidentNormal == -1);
m_clippEdgeNormal[count] = ptr->m_incidentNormal;
m_localPoly[count] = points[ptr->m_incidentVertex];
count++;
ptr = ptr->m_next;
} while (ptr != first);
} else {
do {
m_localPoly[count] = points[ptr->m_incidentVertex];
count++;
ptr = ptr->m_next;
} while (ptr != first);
}
m_count = count;
}
dgVector dgCollisionMesh::dgCollisionConvexPolygon::ClosestDistanceToTriangle(const dgVector &point, const dgVector &p0, const dgVector &p1, const dgVector &p2, bool &isEdge) const {
const dgVector p10(p1 - p0);
const dgVector p20(p2 - p0);
const dgVector p_p0(point - p0);
isEdge = true;
dgFloat32 alpha1 = p10 % p_p0;
dgFloat32 alpha2 = p20 % p_p0;
if ((alpha1 <= dgFloat32(0.0f)) && (alpha2 <= dgFloat32(0.0f))) {
return p0;
}
dgVector p_p1(point - p1);
dgFloat32 alpha3 = p10 % p_p1;
dgFloat32 alpha4 = p20 % p_p1;
if ((alpha3 >= dgFloat32(0.0f)) && (alpha4 <= alpha3)) {
return p1;
}
dgFloat32 vc = alpha1 * alpha4 - alpha3 * alpha2;
if ((vc <= dgFloat32(0.0f)) && (alpha1 >= dgFloat32(0.0f)) && (alpha3 <= dgFloat32(0.0f))) {
dgFloat32 t = alpha1 / (alpha1 - alpha3);
NEWTON_ASSERT(t >= dgFloat32(0.0f));
NEWTON_ASSERT(t <= dgFloat32(1.0f));
return p0 + p10.Scale(t);
}
dgVector p_p2(point - p2);
dgFloat32 alpha5 = p10 % p_p2;
dgFloat32 alpha6 = p20 % p_p2;
if ((alpha6 >= dgFloat32(0.0f)) && (alpha5 <= alpha6)) {
return p2;
}
dgFloat32 vb = alpha5 * alpha2 - alpha1 * alpha6;
if ((vb <= dgFloat32(0.0f)) && (alpha2 >= dgFloat32(0.0f)) && (alpha6 <= dgFloat32(0.0f))) {
dgFloat32 t = alpha2 / (alpha2 - alpha6);
NEWTON_ASSERT(t >= dgFloat32(0.0f));
NEWTON_ASSERT(t <= dgFloat32(1.0f));
return p0 + p20.Scale(t);
}
dgFloat32 va = alpha3 * alpha6 - alpha5 * alpha4;
if ((va <= dgFloat32(0.0f)) && ((alpha4 - alpha3) >= dgFloat32(0.0f)) && ((alpha5 - alpha6) >= dgFloat32(0.0f))) {
dgFloat32 t = (alpha4 - alpha3) / ((alpha4 - alpha3) + (alpha5 - alpha6));
NEWTON_ASSERT(t >= dgFloat32(0.0f));
NEWTON_ASSERT(t <= dgFloat32(1.0f));
return p1 + (p2 - p1).Scale(t);
}
isEdge = false;
dgFloat32 den = float(1.0f) / (va + vb + vc);
dgFloat32 t = vb * den;
dgFloat32 s = vc * den;
NEWTON_ASSERT(t >= dgFloat32(0.0f));
NEWTON_ASSERT(s >= dgFloat32(0.0f));
NEWTON_ASSERT(t <= dgFloat32(1.0f));
NEWTON_ASSERT(s <= dgFloat32(1.0f));
return p0 + p10.Scale(t) + p20.Scale(s);
}
bool dgCollisionMesh::dgCollisionConvexPolygon::PointToPolygonDistance(const dgVector &p, dgFloat32 radius, dgVector &out, bool &isEdge) {
dgFloat32 minDist = dgFloat32(1.0e20f);
m_localPoly[0] = dgVector(&m_vertex[m_index[0] * m_stride]);
m_localPoly[1] = dgVector(&m_vertex[m_index[1] * m_stride]);
// m_localPoly[2] = dgVector (&m_vertex[m_index[2] * m_stride]);
isEdge = true;
dgVector closestPoint(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
for (dgInt32 i2 = 2; i2 < m_count; i2++) {
m_localPoly[i2] = dgVector(&m_vertex[m_index[i2] * m_stride]);
bool tmpIsEdge;
const dgVector q(ClosestDistanceToTriangle(p, m_localPoly[0], m_localPoly[i2 - 1], m_localPoly[i2], tmpIsEdge));
isEdge &= tmpIsEdge;
const dgVector error(q - p);
dgFloat32 dist = error % error;
if (dist < minDist) {
minDist = dist;
closestPoint = q;
}
}
if (minDist > (radius * radius)) {
return false;
}
CalculateNormal();
out = closestPoint;
return true;
}
bool dgCollisionMesh::dgCollisionConvexPolygon::DistanceToOrigen(const dgMatrix &matrix, const dgVector &scale, dgFloat32 radius, dgVector &out, bool &isEdge) {
dgFloat32 minDist = dgFloat32(1.0e20f);
m_localPoly[0] = scale.CompProduct(matrix.TransformVector(dgVector(&m_vertex[m_index[0] * m_stride])));
m_localPoly[1] = scale.CompProduct(matrix.TransformVector(dgVector(&m_vertex[m_index[1] * m_stride])));
isEdge = true;
dgVector origin(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
dgVector closestPoint(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
for (dgInt32 i2 = 2; i2 < m_count; i2++) {
m_localPoly[i2] = scale.CompProduct(matrix.TransformVector(dgVector(&m_vertex[m_index[i2] * m_stride])));
bool tmpIsEdge;
const dgVector q(ClosestDistanceToTriangle(origin, m_localPoly[0], m_localPoly[i2 - 1], m_localPoly[i2], tmpIsEdge));
isEdge &= tmpIsEdge;
const dgVector error(q - origin);
dgFloat32 dist = error % error;
if (dist < minDist) {
minDist = dist;
closestPoint = q;
}
}
if (minDist > (radius * radius)) {
return false;
}
// CalculateNormal();
m_localPoly[0] = dgVector(&m_vertex[m_index[0] * m_stride]);
m_localPoly[1] = dgVector(&m_vertex[m_index[1] * m_stride]);
m_localPoly[2] = dgVector(&m_vertex[m_index[2] * m_stride]);
dgVector e10(m_localPoly[1] - m_localPoly[0]);
dgVector e21(m_localPoly[2] - m_localPoly[1]);
dgVector normal(e10 * e21);
NEWTON_ASSERT((normal % normal) > dgFloat32(0.0f));
m_normal = normal.Scale(dgRsqrt(normal % normal + dgFloat32(1.0e-24f)));
out = closestPoint;
return true;
}
// given an edge of a polygon and a moving sphere, find the first contact the sphere
// makes with the edge, if any. note that hit_time must be primed with a value of 1
// before calling this function the first time. it will then maintain the closest
// collision in subsequent calls.
//
// xs0: start point (center) of sphere
// vs: path of sphere during frame
// rad: radius of sphere
// v0: vertex #1 of the edge
// v1: vertex #2 of the edge
// hit_time: (OUT) time at which sphere collides with polygon edge
// hit_point: (OUT) point on edge that is hit
//
// returns - whether the edge (or it's vertex) was hit
dgFloat32 dgCollisionMesh::dgCollisionConvexPolygon::MovingSphereToEdgeContact(const dgVector &center, const dgVector &veloc, dgFloat32 radius, const dgVector &v0, const dgVector &v1, dgVector &contactOutOnLine) const {
dgVector ve(v1 - v0);
dgVector delta(center - v0);
dgFloat32 delta_dot_ve = delta % ve;
dgFloat32 delta_dot_vs = delta % veloc;
dgFloat32 delta_sqr = delta % delta;
dgFloat32 ve_dot_vs = ve % veloc;
dgFloat32 ve_sqr = ve % ve;
dgFloat32 vs_sqr = veloc % veloc;
// dgFloat32 temp;
// position of the collision along the edge is given by: xe = v0 + ve*s, where s is
// in the range [0,1]. position of sphere along its path is given by:
// xs = xs + vs*t, where t is in the range [0,1]. t is time, but s is arbitrary.
//
// solve simultaneous equations
// (1) distance between edge and sphere center must be sphere radius
// (2) line between sphere center and edge must be perpendicular to edge
//
// (1) (xe - xs)*(xe - xs) = rad*rad
// (2) (xe - xs) * ve = 0
//
dgFloat32 t = dgFloat32(-1.0f);
dgFloat32 A = ve_dot_vs * ve_dot_vs - ve_sqr * vs_sqr;
if (dgAbsf(A) > dgFloat32(1.0e-3f)) {
dgFloat32 B = dgFloat32(2.0f) * (delta_dot_ve * ve_dot_vs - delta_dot_vs * ve_sqr);
dgFloat32 C = delta_dot_ve * delta_dot_ve + radius * radius * ve_sqr - delta_sqr * ve_sqr;
dgFloat32 discriminant = B * B - dgFloat32(4.0f) * A * C;
if (discriminant > dgFloat32(0.0f)) {
discriminant = dgSqrt(discriminant);
dgFloat32 t1 = dgFloat32(0.5f) * (-B + discriminant) / A;
dgFloat32 t2 = dgFloat32(0.5f) * (-B - discriminant) / A;
// sort root1 and root2, use the earliest intersection. the larger root
// corresponds to the final contact of the sphere with the edge on its
// way out.
t1 = GetMin(t1, t2);
// find sphere and edge positions
dgVector temp_sphere_hit(center + veloc.Scale(t1));
if (t1 >= dgFloat32(0.0f)) {
dgFloat32 edge_param = ((temp_sphere_hit - v0) % ve) / ve_sqr;
if ((edge_param >= dgFloat32(0.0f)) && (edge_param <= dgFloat32(1.0f))) {
t = t1;
contactOutOnLine = v0 + ve.Scale(edge_param);
}
}
}
}
return t;
}
dgFloat32 dgCollisionMesh::dgCollisionConvexPolygon::MovingPointToPolygonContact(const dgVector &p, const dgVector &veloc, dgFloat32 radius, dgContactPoint &contact) {
m_localPoly[0] = dgVector(&m_vertex[m_index[0] * m_stride]);
m_localPoly[1] = dgVector(&m_vertex[m_index[1] * m_stride]);
m_localPoly[2] = dgVector(&m_vertex[m_index[2] * m_stride]);
CalculateNormal();
if ((m_normal % veloc) >= dgFloat32(0.0f)) {
return dgFloat32(-1.0f);
}
dgFloat32 timestep = dgFloat32(-1.0f);
dgVector closestPoint(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
dgFloat32 minDist = dgFloat32(1.0e20f);
for (dgInt32 j = 2; j < m_count; j++) {
bool tmpIsEdge;
m_localPoly[j] = dgVector(&m_vertex[m_index[j] * m_stride]);
const dgVector q(ClosestDistanceToTriangle(p, m_localPoly[0], m_localPoly[j - 1], m_localPoly[j], tmpIsEdge));
const dgVector error(q - p);
dgFloat32 dist = error % error;
if (dist < minDist) {
minDist = dist;
closestPoint = q;
}
}
if (minDist <= (radius * radius)) {
dgVector dp(p - closestPoint);
dgFloat32 dist2 = dp % dp;
if (dist2 > dgFloat32(0.0f)) {
NEWTON_ASSERT(dist2 > dgFloat32(0.0f));
dgFloat32 dist2Inv = dgRsqrt(dist2);
dgFloat32 side = dist2 * dist2Inv - radius;
if (side < (-DG_RESTING_CONTACT_PENETRATION)) {
dgVector normal(dp.Scale(dist2Inv));
side = (dgAbsf(side) - DG_IMPULSIVE_CONTACT_PENETRATION);
if (side < dgFloat32(0.0f)) {
side = dgFloat32(0.0f);
}
timestep = dgFloat32(0.0f);
contact.m_point = closestPoint;
contact.m_normal = normal;
contact.m_penetration = side;
contact.m_isEdgeContact = 0;
}
}
}
// this method from Paul Nettle for moving sphere against polygon,
// it is faster and seems to be better than my own method general Minskonsky volume, so I will spacial case the shape for performance
// I am still having round off error I will give another try and see what is wrong
if (timestep < 0.0f) {
dgFloat32 scale = dgFloat32(1.0f) / dgSqrt(veloc % veloc);
dgVector vdir = veloc.Scale(scale);
NEWTON_ASSERT(dgAbsf(m_normal % vdir) > dgFloat32(0.0f));
dgVector supportPoint(p - m_normal.Scale(radius));
dgFloat32 timeToImpact = -(m_normal % (supportPoint - m_localPoly[0])) / (m_normal % vdir);
dgVector point(supportPoint + vdir.Scale(timeToImpact));
dgVector closestPointN(point);
dgFloat32 minDistance = dgFloat32(1.0e20f);
bool isEdgeFlag = true;
for (int i = 2; i < m_count; i++) {
bool tmpIsEdge;
const dgVector q(ClosestDistanceToTriangle(point, m_localPoly[0], m_localPoly[i - 1], m_localPoly[i], tmpIsEdge));
isEdgeFlag &= tmpIsEdge;
const dgVector error(q - point);
dgFloat32 dist = error % error;
if (dist < minDistance) {
minDistance = dist;
closestPointN = q;
}
}
if (!isEdgeFlag) {
NEWTON_ASSERT(minDistance < dgFloat32(1.0e-3f));
timestep = GetMax(timeToImpact, dgFloat32(0.0f));
contact.m_normal = m_normal;
contact.m_penetration = dgFloat32(0.0f);
contact.m_isEdgeContact = 0;
contact.m_point = p - m_normal.Scale(radius);
contact.m_point = closestPointN;
} else {
/*
NEWTON_ASSERT (isEdge);
dgVector dp (closestPointN - p);
// this does not really work ( goidnm back to my old method
// has I finally found the bug, when V is very large, then
// v % v introduces a very large round of error that completely invalidates the meaning of (b * b - 4 * a * c)
// so the solution is to use a normalize value for v, and the final t is the scale by the inverse magnitude of veloc
//dgFloat32 a = veloc % veloc;
//dgFloat32 b = - dgFloat32 (2.0f) * (dp % veloc);
dgFloat32 a = dgFloat32 (1.0f);
dgFloat32 b = - dgFloat32 (2.0f) * (dp % vdir);
dgFloat32 c = dp % dp - radius * radius;
dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc >= dgFloat32 (0.0f)) {
desc = dgSqrt (desc);
dgFloat32 den = dgFloat32 (0.5f) / a;
dgFloat32 t0 = den * (-b - desc);
dgFloat32 t1 = den * (-b + desc);
dgFloat32 t = GetMin (t0, t1);
if (t >= 0.0f) {
timestep = t;
contact.m_penetration = dgFloat32 (0.0f);
contact.m_isEdgeContact = isEdgeFlag ? 1 : 0;
contact.m_point = closestPointN;
dgVector n (p - (closestPointN - vdir.Scale (t)));
contact.m_normal = n.Scale (dgRsqrt (n % n));
}
}
*/
bool edgefound = false;
dgFloat32 mint = dgFloat32(1.0e10f);
dgInt32 i0 = m_count - 1;
for (dgInt32 i1 = 0; i1 < m_count; i1++) {
dgVector tmp;
dgFloat32 t = MovingSphereToEdgeContact(p, vdir, radius, m_localPoly[i0], m_localPoly[i1], tmp);
if ((t > dgFloat32(0.0f)) && (t < mint)) {
mint = t;
edgefound = true;
closestPointN = tmp;
}
i0 = i1;
}
if (edgefound) {
timestep = mint;
contact.m_penetration = dgFloat32(0.0f);
contact.m_isEdgeContact = 1;
contact.m_point = closestPointN;
dgVector n(p - (closestPointN - vdir.Scale(mint)));
contact.m_normal = n.Scale(dgRsqrt(n % n));
}
}
timestep *= scale;
}
return timestep;
}
bool dgCollisionMesh::dgCollisionConvexPolygon::IsEdgeIntersection() const {
return m_isEdgeIntersection ? true : false;
}
dgInt32 dgCollisionMesh::dgCollisionConvexPolygon::CalculatePlaneIntersectionSimd(const dgVector &normalIn, const dgVector &origin, dgVector *const contactsOut) const {
dgVector normal(normalIn);
dgInt32 count = 0;
dgFloat32 maxDist = dgFloat32(1.0f);
dgFloat32 projectFactor = m_normal % normal;
if (projectFactor < dgFloat32(0.0f)) {
projectFactor *= dgFloat32(-1.0f);
normal = normal.Scale(dgFloat32(-1.0f));
}
if (projectFactor > dgFloat32(0.9999f)) {
m_isEdgeIntersection = 0;
for (dgInt32 i = 0; i < m_count; i++) {
contactsOut[count] = m_localPoly[i];
count++;
}
} else if (projectFactor > dgFloat32(0.1736f)) {
m_isEdgeIntersection = 1;
maxDist = dgFloat32(0.0f);
dgPlane plane(normal, -(normal % origin));
dgVector p0(m_localPoly[m_count - 1]);
dgFloat32 side0 = plane.Evalue(p0);
for (dgInt32 i = 0; i < m_count; i++) {
dgVector p1(m_localPoly[i]);
dgFloat32 side1 = plane.Evalue(p1);
if (side0 > dgFloat32(0.0f)) {
maxDist = GetMax(maxDist, side0);
contactsOut[count] = p0 - plane.Scale(side0);
count++;
if (count > 1) {
dgVector edgeSegment(contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32(1.0e-8f)) {
count--;
}
}
if (side1 <= dgFloat32(0.0f)) {
dgVector dp(p1 - p0);
dgFloat32 t = plane % dp;
NEWTON_ASSERT(dgAbsf(t) >= dgFloat32(0.0f));
if (dgAbsf(t) < dgFloat32(1.0e-8f)) {
t = GetSign(t) * dgFloat32(1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale(side0 / t);
count++;
if (count > 1) {
dgVector edgeSegment(contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32(1.0e-8f)) {
count--;
}
}
}
} else if (side1 > dgFloat32(0.0f)) {
dgVector dp(p1 - p0);
dgFloat32 t = plane % dp;
NEWTON_ASSERT(dgAbsf(t) >= dgFloat32(0.0f));
if (dgAbsf(t) < dgFloat32(1.0e-8f)) {
t = GetSign(t) * dgFloat32(1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale(side0 / t);
count++;
if (count > 1) {
dgVector edgeSegment(contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32(1.0e-8f)) {
count--;
}
}
}
side0 = side1;
p0 = p1;
}
}
if (count > 1) {
if (maxDist < dgFloat32(1.0e-3f)) {
dgFloat32 proj;
dgFloat32 maxProjection;
dgFloat32 minProjection;
dgVector maxPoint(contactsOut[0]);
dgVector minPoint(contactsOut[0]);
dgVector lineDir(m_normal * normal);
proj = contactsOut[0] % lineDir;
maxProjection = proj;
minProjection = proj;
for (dgInt32 i = 1; i < count; i++) {
proj = contactsOut[i] % lineDir;
if (proj > maxProjection) {
maxProjection = proj;
maxPoint = contactsOut[i];
}
if (proj < minProjection) {
minProjection = proj;
minPoint = contactsOut[i];
}
}
contactsOut[0] = maxPoint;
contactsOut[1] = minPoint;
count = 2;
}
dgVector error(contactsOut[count - 1] - contactsOut[0]);
if ((error % error) < dgFloat32(1.0e-8f)) {
count--;
}
}
#if 0 && defined(_DEBUG) // NEWTON_ASSERT is disabled so this whole calculation is useless
if (count > 1) {
dgInt32 j;
j = count - 1;
for (dgInt32 i = 0; i < count; i++) {
dgVector error(contactsOut[i] - contactsOut[j]);
NEWTON_ASSERT((error % error) > dgFloat32(1.0e-20f));
j = i;
}
if (count >= 3) {
dgVector n(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
dgVector e0(contactsOut[1] - contactsOut[0]);
for (dgInt32 i = 2; i < count; i++) {
dgVector e1(contactsOut[i] - contactsOut[0]);
n += e0 * e1;
e0 = e1;
}
n = n.Scale(dgFloat32(1.0f) / dgSqrt(n % n));
dgFloat32 val = n % normal;
NEWTON_ASSERT(val > dgFloat32(0.9f));
}
}
#endif
return count;
}
dgInt32 dgCollisionMesh::dgCollisionConvexPolygon::CalculatePlaneIntersection(const dgVector &normalIn, const dgVector &origin, dgVector *const contactsOut) const {
dgVector normal(normalIn);
dgInt32 count = 0;
dgFloat32 maxDist = dgFloat32(1.0f);
dgFloat32 projectFactor = m_normal % normal;
if (projectFactor < dgFloat32(0.0f)) {
projectFactor *= dgFloat32(-1.0f);
normal = normal.Scale(dgFloat32(-1.0f));
}
if (projectFactor > dgFloat32(0.9999f)) {
m_isEdgeIntersection = 0;
for (dgInt32 i = 0; i < m_count; i++) {
contactsOut[count] = m_localPoly[i];
count++;
}
#if 0 && defined(_DEBUG) // NEWTON_ASSERT is disabled so this whole calculation is useless
dgInt32 j = count - 1;
for (dgInt32 i = 0; i < count; i++) {
dgVector error(contactsOut[i] - contactsOut[j]);
NEWTON_ASSERT((error % error) > dgFloat32(1.0e-20f));
j = i;
}
#endif
} else if (projectFactor > dgFloat32(0.1736f)) {
m_isEdgeIntersection = 1;
maxDist = dgFloat32(0.0f);
dgPlane plane(normal, -(normal % origin));
dgVector p0(m_localPoly[m_count - 1]);
dgFloat32 side0 = plane.Evalue(p0);
for (dgInt32 i = 0; i < m_count; i++) {
dgVector p1(m_localPoly[i]);
dgFloat32 side1 = plane.Evalue(p1);
if (side0 > dgFloat32(0.0f)) {
maxDist = GetMax(maxDist, side0);
contactsOut[count] = p0 - plane.Scale(side0);
count++;
if (count > 1) {
dgVector edgeSegment(contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32(1.0e-8f)) {
count--;
}
}
if (side1 <= dgFloat32(0.0f)) {
dgVector dp(p1 - p0);
dgFloat32 t = plane % dp;
NEWTON_ASSERT(dgAbsf(t) >= dgFloat32(0.0f));
if (dgAbsf(t) < dgFloat32(1.0e-8f)) {
t = GetSign(t) * dgFloat32(1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale(side0 / t);
count++;
if (count > 1) {
dgVector edgeSegment(contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32(1.0e-8f)) {
count--;
}
}
}
} else if (side1 > dgFloat32(0.0f)) {
dgVector dp(p1 - p0);
dgFloat32 t = plane % dp;
NEWTON_ASSERT(dgAbsf(t) >= dgFloat32(0.0f));
if (dgAbsf(t) < dgFloat32(1.0e-8f)) {
t = GetSign(t) * dgFloat32(1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale(side0 / t);
count++;
if (count > 1) {
dgVector edgeSegment(contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32(1.0e-8f)) {
count--;
}
}
}
side0 = side1;
p0 = p1;
}
}
if (count > 1) {
if (maxDist < dgFloat32(1.0e-3f)) {
dgFloat32 proj;
dgFloat32 maxProjection;
dgFloat32 minProjection;
dgVector maxPoint(contactsOut[0]);
dgVector minPoint(contactsOut[0]);
dgVector lineDir(m_normal * normal);
proj = contactsOut[0] % lineDir;
maxProjection = proj;
minProjection = proj;
for (dgInt32 i = 1; i < count; i++) {
proj = contactsOut[i] % lineDir;
if (proj > maxProjection) {
maxProjection = proj;
maxPoint = contactsOut[i];
}
if (proj < minProjection) {
minProjection = proj;
minPoint = contactsOut[i];
}
}
contactsOut[0] = maxPoint;
contactsOut[1] = minPoint;
count = 2;
}
dgVector error(contactsOut[count - 1] - contactsOut[0]);
if ((error % error) < dgFloat32(1.0e-8f)) {
count--;
}
}
#if 0 && defined(_DEBUG) // NEWTON_ASSERT is disabled so this whole calculation is useless
if (count > 1) {
dgInt32 j = count - 1;
for (dgInt32 i = 0; i < count; i++) {
dgVector error(contactsOut[i] - contactsOut[j]);
NEWTON_ASSERT((error % error) > dgFloat32(1.0e-20f));
j = i;
}
if (count >= 3) {
dgVector n(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
dgVector e0(contactsOut[1] - contactsOut[0]);
for (dgInt32 i = 2; i < count; i++) {
dgVector e1(contactsOut[i] - contactsOut[0]);
n += e0 * e1;
e0 = e1;
}
n = n.Scale(dgFloat32(1.0f) / dgSqrt(n % n));
dgFloat32 val = n % normal;
NEWTON_ASSERT(val > dgFloat32(0.9f));
}
}
#endif
return count;
}
dgCollisionMesh::dgCollisionMesh(dgMemoryAllocator *const allocator, dgCollisionID type)
: dgCollision(allocator, 0, dgGetIdentityMatrix(), type) {
m_allocator = allocator;
m_rtti |= dgCollisionMesh_RTTI;
for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i++) {
m_polygon[i] = new (allocator) dgCollisionConvexPolygon(allocator);
}
m_debugCallback = NULL;
// m_userRayCastCallback = NULL;
SetCollisionBBox(dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)));
}
// dgCollisionMesh::dgCollisionMesh (dgDeserialize deserialization, void* userData, const dgPolysoupCreation& data)
dgCollisionMesh::dgCollisionMesh(dgWorld *const world, dgDeserialize deserialization, void *const userData)
: dgCollision(world, deserialization, userData) {
m_rtti |= dgCollisionMesh_RTTI;
for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i++) {
m_polygon[i] = new (world->GetAllocator()) dgCollisionConvexPolygon(world->GetAllocator());
}
m_debugCallback = NULL;
// m_userRayCastCallback = NULL;
SetCollisionBBox(dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)),
dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f)));
}
dgCollisionMesh::~dgCollisionMesh() {
for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i++) {
m_polygon[i]->Release();
}
}
void dgCollisionMesh::SetCollisionBBox(const dgVector &p0, const dgVector &p1) {
NEWTON_ASSERT(p0.m_x <= p1.m_x);
NEWTON_ASSERT(p0.m_y <= p1.m_y);
NEWTON_ASSERT(p0.m_z <= p1.m_z);
m_boxSize = (p1 - p0).Scale(dgFloat32(0.5f));
m_boxOrigin = (p1 + p0).Scale(dgFloat32(0.5f));
}
dgInt32 dgCollisionMesh::CalculateSignature() const {
NEWTON_ASSERT(0);
return 0;
}
void *dgCollisionMesh::GetUserData() const {
return NULL;
}
/*
void dgCollisionMesh::SetCallBack___ (void *callBack)
{
m_collideCallback = (OnPolygonSoupCollideCallback) callBack;
}
void *dgCollisionMesh::GetCallBack___ () const
{
return (void*) m_collideCallback;
}
*/
void dgCollisionMesh::SetCollisionCallback(dgCollisionMeshCollisionCallback debugCallback) {
m_debugCallback = debugCallback;
}
#ifdef DG_DEBUG_AABB
dgVector dgCollisionMesh::BoxSupportMapping(const dgVector &dir) const {
return dgVector(dir.m_x < dgFloat32(0.0f) ? m_p0.m_x : m_p1.m_x,
dir.m_y < dgFloat32(0.0f) ? m_p0.m_y : m_p1.m_y,
dir.m_z < dgFloat32(0.0f) ? m_p0.m_z : m_p1.m_z, dgFloat32(0.0f));
}
#endif
void dgCollisionMesh::CalcAABB(const dgMatrix &matrix, dgVector &p0, dgVector &p1) const {
dgVector origin(matrix.TransformVector(m_boxOrigin));
dgVector size(m_boxSize.m_x * dgAbsf(matrix[0][0]) + m_boxSize.m_y * dgAbsf(matrix[1][0]) + m_boxSize.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,
m_boxSize.m_x * dgAbsf(matrix[0][1]) + m_boxSize.m_y * dgAbsf(matrix[1][1]) + m_boxSize.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,
m_boxSize.m_x * dgAbsf(matrix[0][2]) + m_boxSize.m_y * dgAbsf(matrix[1][2]) + m_boxSize.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
dgFloat32(0.0f));
p0 = origin - size;
p1 = origin + size;
#ifdef DG_DEBUG_AABB
dgInt32 i;
dgVector q0;
dgVector q1;
dgMatrix trans(matrix.Transpose());
for (i = 0; i < 3; i++) {
q0[i] = matrix.m_posit[i] + matrix.RotateVector(BoxSupportMapping(trans[i].Scale(-1.0f)))[i];
q1[i] = matrix.m_posit[i] + matrix.RotateVector(BoxSupportMapping(trans[i]))[i];
}
dgVector err0(p0 - q0);
dgVector err1(p1 - q1);
dgFloat32 err;
err = GetMax(size.m_x, size.m_y, size.m_z) * 0.5f;
NEWTON_ASSERT((err0 % err0) < err);
NEWTON_ASSERT((err1 % err1) < err);
#endif
}
void dgCollisionMesh::CalcAABBSimd(const dgMatrix &matrix, dgVector &p0, dgVector &p1) const {
CalcAABB(matrix, p0, p1);
}
dgInt32 dgCollisionMesh::CalculatePlaneIntersection(const dgFloat32 *vertex, const dgInt32 *index, dgInt32 indexCount, dgInt32 stride, const dgPlane &localPlane, dgVector *const contactsOut) const {
dgInt32 i;
dgInt32 j;
dgInt32 count;
dgFloat32 t;
dgFloat32 side0;
dgFloat32 side1;
count = 0;
j = index[indexCount - 1] * stride;
dgVector p0(&vertex[j]);
side0 = localPlane.Evalue(p0);
for (i = 0; i < indexCount; i++) {
j = index[i] * stride;
dgVector p1(&vertex[j]);
side1 = localPlane.Evalue(p1);
if (side0 < dgFloat32(0.0f)) {
if (side1 >= dgFloat32(0.0f)) {
dgVector dp(p1 - p0);
t = localPlane % dp;
NEWTON_ASSERT(dgAbsf(t) >= dgFloat32(0.0f));
if (dgAbsf(t) < dgFloat32(1.0e-8f)) {
t = GetSign(t) * dgFloat32(1.0e-8f);
}
NEWTON_ASSERT(0);
contactsOut[count] = p0 - dp.Scale(side0 / t);
count++;
}
} else if (side1 <= dgFloat32(0.0f)) {
dgVector dp(p1 - p0);
t = localPlane % dp;
NEWTON_ASSERT(dgAbsf(t) >= dgFloat32(0.0f));
if (dgAbsf(t) < dgFloat32(1.0e-8f)) {
t = GetSign(t) * dgFloat32(1.0e-8f);
}
NEWTON_ASSERT(0);
contactsOut[count] = p0 - dp.Scale(side0 / t);
count++;
}
side0 = side1;
p0 = p1;
}
return count;
}
dgVector dgCollisionMesh::CalculateVolumeIntegral(const dgMatrix &globalMatrix__, GetBuoyancyPlane buoyancuPlane__, void *context__) const {
return dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
}
// void dgCollisionMesh::DebugCollision (const dgBody& myBody, DebugCollisionMeshCallback callback) const
void dgCollisionMesh::DebugCollision(const dgMatrix &matrixPtr, OnDebugCollisionMeshCallback callback, void *const userData) const {
NEWTON_ASSERT(0);
}
dgFloat32 dgCollisionMesh::GetVolume() const {
// NEWTON_ASSERT (0);
return dgFloat32(0.0f);
}
dgFloat32 dgCollisionMesh::GetBoxMinRadius() const {
return dgFloat32(0.0f);
}
dgFloat32 dgCollisionMesh::GetBoxMaxRadius() const {
return dgFloat32(0.0f);
}
void dgCollisionMesh::CalculateInertia(dgVector &inertia, dgVector &origin) const {
inertia.m_x = dgFloat32(0.0f);
inertia.m_y = dgFloat32(0.0f);
inertia.m_z = dgFloat32(0.0f);
origin.m_x = dgFloat32(0.0f);
origin.m_y = dgFloat32(0.0f);
origin.m_z = dgFloat32(0.0f);
}
void dgCollisionMesh::GetCollisionInfo(dgCollisionInfo *info) const {
NEWTON_ASSERT(0);
// dgCollision::GetCollisionInfo(info);
// info->m_offsetMatrix = GetOffsetMatrix();
// info->m_collisionType = m_collsionId;
}
void dgCollisionMesh::Serialize(dgSerialize callback, void *const userData) const {
NEWTON_ASSERT(0);
}
dgVector dgCollisionMesh::SupportVertex(const dgVector &dir) const {
NEWTON_ASSERT(0);
return dgVector(0, 0, 0, 0);
}
bool dgCollisionMesh::OOBBTest(const dgMatrix &matrix, const dgCollisionConvex *const shape, void *const cacheOrder) const {
NEWTON_ASSERT(0);
return true;
}
Reference in New Issue View Git Blame Copy Permalink