dgInt32 dgCollisionChamferCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 count = 0;
const dgFloat32 inclination = dgFloat32 (0.9999f);
if (normal.m_x < -inclination) {
dgMatrix matrix(normal);
dgFloat32 x = dgSqrt (dgMax (m_height * m_height - origin.m_x * origin.m_x, dgFloat32 (0.0f)));
matrix.m_posit.m_x = origin.m_x;
dgVector scale(m_radius + x);
const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]);
for (dgInt32 i = 0; i < n; i++) {
contactsOut[i] = matrix.TransformVector(m_unitCircle[i] * scale) & dgVector::m_triplexMask;
}
count = RectifyConvexSlice(n, normal, contactsOut);
} else if (normal.m_x > inclination) {
dgMatrix matrix(normal);
dgFloat32 x = dgSqrt (dgMax (m_height * m_height - origin.m_x * origin.m_x, dgFloat32 (0.0f)));
matrix.m_posit.m_x = origin.m_x;
dgVector scale(m_radius + x);
const int n = sizeof (m_unitCircle) / sizeof (m_unitCircle[0]);
for (dgInt32 i = 0; i < n; i++) {
contactsOut[i] = matrix.TransformVector(m_unitCircle[i] * scale) & dgVector::m_triplexMask;
}
count = RectifyConvexSlice(n, normal, contactsOut);
} else {
count = 1;
contactsOut[0] = SupportVertex (normal, NULL);
}
return count;
}
dgInt32 dgCollisionChamferCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgInt32 count = 0;
if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) {
count = 1;
contactsOut[0] = SupportVertex (normal, NULL);
} else {
count = dgCollisionConvex::CalculatePlaneIntersection (normal, origin, contactsOut, normalSign);
}
return count;
}
dgVector dgCollisionTaperedCylinder::ConvexConicSupporVertex (const dgVector& point, const dgVector& dir) const
{
dgVector p (SupportVertex(dir, NULL));
if (dgAbsf(dir.m_x) > dgFloat32 (0.9997f)) {
p.m_y = point.m_y;
p.m_z = point.m_z;
} else {
dgVector n (dir.m_x, dgSqrt (dir.m_y * dir.m_y + dir.m_z * dir.m_z), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgFloat32 project = n % m_dirVector;
if (project > dgFloat32 (0.9998f)) {
p.m_x = point.m_x;
}
}
return p;
}
dgVector dgCollisionTaperedCapsule::ConvexConicSupporVertex (const dgVector& point, const dgVector& dir) const
{
dgAssert (dgAbsf (dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgVector p (SupportVertex(dir, NULL));
dgVector n (dir.m_x, dgSqrt (dir.m_y * dir.m_y + dir.m_z * dir.m_z), dgFloat32 (0.0), dgFloat32 (0.0f));
dgAssert (dgAbsf (n % n - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 project = m_sideNormal % n;
if (project > dgFloat32 (0.9998f)) {
dgFloat32 t = dgFloat32 (0.5f) * (point.m_x + m_height) / m_height;
dgFloat32 r = m_radio1 + (m_radio0 - m_radio1) * t;
p = dir.Scale3 (r);
p.m_x += point.m_x;
}
return p;
}
dgInt32 dgConvexHull4d::InitVertexArray(dgHullVector* const points, const dgBigVector* const vertexCloud, dgInt32 count, void* const memoryPool, dgInt32 maxMemSize)
{
for (dgInt32 i = 0; i < count; i ++) {
points[i] = vertexCloud[i];
points[i].m_index = i;
points[i].m_mark = 0;
}
dgSort(points, count, ConvexCompareVertex);
dgInt32 indexCount = 0;
for (int i = 1; i < count; i ++) {
for (; i < count; i ++) {
if (ConvexCompareVertex (&points[indexCount], &points[i], NULL)) {
indexCount ++;
points[indexCount] = points[i];
break;
}
}
}
count = indexCount + 1;
if (count < 4) {
m_count = 0;
return count;
}
dgAABBPointTree4d* tree = BuildTree (NULL, points, count, 0, (dgInt8**) &memoryPool, maxMemSize);
dgBigVector boxSize (tree->m_box[1] - tree->m_box[0]);
boxSize.m_w = dgFloat64 (0.0f);
m_diag = dgFloat32 (sqrt (boxSize.DotProduct4(boxSize).m_x));
m_points[4].m_x = dgFloat64 (0.0f);
dgHullVector* const convexPoints = &m_points[0];
dgStack<dgBigVector> normalArrayPool (256);
dgBigVector* const normalArray = &normalArrayPool[0];
dgInt32 normalCount = BuildNormalList (&normalArray[0]);
dgInt32 index = SupportVertex (&tree, points, normalArray[0]);
convexPoints[0] = points[index];
points[index].m_mark = 1;
bool validTetrahedrum = false;
dgBigVector e1 (dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f), dgFloat64 (0.0f)) ;
for (dgInt32 i = 1; i < normalCount; i ++) {
dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
dgAssert (index >= 0);
e1 = points[index] - convexPoints[0];
e1.m_w = dgFloat64 (0.0f);
dgFloat64 error2 = e1.DotProduct4(e1).m_x;
if (error2 > (dgFloat32 (1.0e-4f) * m_diag * m_diag)) {
convexPoints[1] = points[index];
points[index].m_mark = 1;
validTetrahedrum = true;
break;
}
}
if (!validTetrahedrum) {
m_count = 0;
return count;
}
dgInt32 bestIndex = -1;
dgFloat64 bestValue = dgFloat64 (1.0f);
validTetrahedrum = false;
dgFloat64 lenght2 = e1.DotProduct4(e1).m_x;
dgBigVector e2(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));;
for (dgInt32 i = 2; i < normalCount; i ++) {
dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
dgAssert (index >= 0);
dgAssert (index < count);
e2 = points[index] - convexPoints[0];
e2.m_w = dgFloat64 (0.0f);
dgFloat64 den = e2.DotProduct4(e2).m_x;
if (fabs (den) > (dgFloat64 (1.0e-6f) * m_diag)) {
den = sqrt (lenght2 * den);
dgFloat64 num = e2.DotProduct4(e1).m_x;
dgFloat64 cosAngle = fabs (num / den);
if (cosAngle < bestValue) {
bestValue = cosAngle;
bestIndex = index;
}
if (cosAngle < 0.9f) {
break;
}
}
}
if (bestValue < dgFloat64 (0.999f)) {
convexPoints[2] = points[bestIndex];
points[bestIndex].m_mark = 1;
validTetrahedrum = true;
}
if (!validTetrahedrum) {
m_count = 0;
return count;
}
validTetrahedrum = false;
dgBigVector e3(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));;
for (dgInt32 i = 3; i < normalCount; i ++) {
dgInt32 index = SupportVertex (&tree, points, normalArray[i]);
dgAssert (index >= 0);
dgAssert (index < count);
e3 = points[index] - convexPoints[0];
e3.m_w = dgFloat64 (0.0f);
dgFloat64 volume = (e1 * e2) % e3;
if (fabs (volume) > (dgFloat64 (1.0e-4f) * m_diag * m_diag * m_diag)) {
convexPoints[3] = points[index];
points[index].m_mark = 1;
validTetrahedrum = true;
break;
}
}
m_count = 4;
if (!validTetrahedrum) {
m_count = 0;
}
return count;
}
dgVector dgCollisionSphere::ConvexConicSupporVertex (const dgVector& point, const dgVector& dir) const
{
return SupportVertex(dir, NULL);
}
dgVector dgCollisionBVH::SupportVertexSpecial(const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert(0);
return SupportVertex(dir, vertexIndex);
}
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);
// _ASSERTE (plane.Evalue(m_boxOrigin) < 0.0f);
dgPlane& tmpPlane = planesArray[planeCount];
tmpPlane = plane;
planesArray[planeCount].m_index = i;
planesArray[planeCount].m_face = face;
planeCount ++;
_ASSERTE (planeCount < (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;
}
}
_ASSERTE (j < m_faceCount);
}
m_boundPlanesCount ++;
}
m_destructionImpulse = dgFloat32 (1.0e20f);
}
dgVector dgCollisionCone::SupportVertexSimd (const dgVector& dir) const
{
return SupportVertex (dir);
}
dgVector dgCollisionChamferCylinder::SupportVertexSimd (const dgVector& dir) const
{
return SupportVertex (dir);
}
dgInt32 dgCollisionBox::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const
{
dgVector support[4];
dgInt32 featureCount = 3;
const dgConvexSimplexEdge** const vertToEdgeMapping = GetVertexToEdgeMapping();
if (vertToEdgeMapping) {
dgInt32 edgeIndex;
//support[0] = SupportVertex (normal.Scale4(normalSign), &edgeIndex);
support[0] = SupportVertex (normal, &edgeIndex);
dgFloat32 dist = normal.DotProduct4(support[0] - point).GetScalar();
if (dist <= DG_IMPULSIVE_CONTACT_PENETRATION) {
dgVector normalAlgin (normal.Abs());
if (!((normalAlgin.m_x > dgFloat32 (0.9999f)) || (normalAlgin.m_y > dgFloat32 (0.9999f)) || (normalAlgin.m_z > dgFloat32 (0.9999f)))) {
// 0.25 degrees
const dgFloat32 tiltAngle = dgFloat32 (0.005f);
const dgFloat32 tiltAngle2 = tiltAngle * tiltAngle ;
dgPlane testPlane (normal, - (normal.DotProduct4(support[0]).GetScalar()));
featureCount = 1;
const dgConvexSimplexEdge* const edge = vertToEdgeMapping[edgeIndex];
const dgConvexSimplexEdge* ptr = edge;
do {
const dgVector& p = m_vertex[ptr->m_twin->m_vertex];
dgFloat32 test1 = testPlane.Evalue(p);
dgVector dist (p - support[0]);
dgFloat32 angle2 = test1 * test1 / (dist.DotProduct4(dist).GetScalar());
if (angle2 < tiltAngle2) {
support[featureCount] = p;
featureCount ++;
}
ptr = ptr->m_twin->m_next;
} while ((ptr != edge) && (featureCount < 3));
}
}
}
dgInt32 count = 0;
switch (featureCount)
{
case 1:
{
contactsOut[0] = support[0] - normal.CompProduct4(normal.DotProduct4(support[0] - point));
count = 1;
break;
}
case 2:
{
contactsOut[0] = support[0] - normal.CompProduct4(normal.DotProduct4(support[0] - point));
contactsOut[1] = support[1] - normal.CompProduct4(normal.DotProduct4(support[1] - point));
count = 2;
break;
}
default:
{
dgFloat32 test[8];
dgAssert(normal.m_w == dgFloat32(0.0f));
dgPlane plane(normal, -(normal.DotProduct4(point).GetScalar()));
for (dgInt32 i = 0; i < 8; i++) {
dgAssert(m_vertex[i].m_w == dgFloat32(0.0f));
test[i] = plane.DotProduct4(m_vertex[i] | dgVector::m_wOne).m_x;
}
dgConvexSimplexEdge* edge = NULL;
for (dgInt32 i = 0; i < dgInt32 (sizeof (m_edgeEdgeMap) / sizeof (m_edgeEdgeMap[0])); i ++) {
dgConvexSimplexEdge* const ptr = m_edgeEdgeMap[i];
dgFloat32 side0 = test[ptr->m_vertex];
dgFloat32 side1 = test[ptr->m_twin->m_vertex];
if ((side0 * side1) < dgFloat32 (0.0f)) {
edge = ptr;
break;
}
}
if (edge) {
if (test[edge->m_vertex] < dgFloat32 (0.0f)) {
edge = edge->m_twin;
}
dgAssert (test[edge->m_vertex] > dgFloat32 (0.0f));
dgConvexSimplexEdge* ptr = edge;
dgConvexSimplexEdge* firstEdge = NULL;
dgFloat32 side0 = test[edge->m_vertex];
do {
dgAssert (m_vertex[ptr->m_twin->m_vertex].m_w == dgFloat32 (0.0f));
dgFloat32 side1 = test[ptr->m_twin->m_vertex];
if (side1 < side0) {
if (side1 < dgFloat32 (0.0f)) {
firstEdge = ptr;
break;
}
side0 = side1;
edge = ptr->m_twin;
ptr = edge;
}
ptr = ptr->m_twin->m_next;
} while (ptr != edge);
if (firstEdge) {
edge = firstEdge;
ptr = edge;
do {
dgVector dp (m_vertex[ptr->m_twin->m_vertex] - m_vertex[ptr->m_vertex]);
dgFloat32 t = plane.DotProduct4(dp).m_x;
if (t >= dgFloat32 (-1.e-24f)) {
t = dgFloat32 (0.0f);
} else {
t = test[ptr->m_vertex] / t;
if (t > dgFloat32 (0.0f)) {
t = dgFloat32 (0.0f);
}
if (t < dgFloat32 (-1.0f)) {
t = dgFloat32 (-1.0f);
}
}
dgAssert (t <= dgFloat32 (0.01f));
dgAssert (t >= dgFloat32 (-1.05f));
contactsOut[count] = m_vertex[ptr->m_vertex] - dp.Scale4 (t);
count ++;
dgConvexSimplexEdge* ptr1 = ptr->m_next;
for (; ptr1 != ptr; ptr1 = ptr1->m_next) {
dgInt32 index0 = ptr1->m_twin->m_vertex;
if (test[index0] >= dgFloat32 (0.0f)) {
dgAssert (test[ptr1->m_vertex] <= dgFloat32 (0.0f));
break;
}
}
dgAssert (ptr != ptr1);
ptr = ptr1->m_twin;
} while ((ptr != edge) && (count < 8));
}
}
}
}
if (count > 2) {
count = RectifyConvexSlice (count, normal, contactsOut);
}
return count;
}
