dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 x = dir.GetScalar();
if (dgAbsf (x) > dgFloat32 (0.9999f)) {
return dgVector ((x > dgFloat32 (0.0f)) ? m_height : - m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
dgVector sideDir (m_yzMask & dir);
sideDir = sideDir.CompProduct4(sideDir.InvMagSqrt());
return sideDir.Scale4(m_radius) + dir.Scale4 (m_height);
}
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 x = dir.GetScalar();
if (dgAbsf (x) > dgFloat32 (0.9999f)) {
dgFloat32 x0 = (x >= dgFloat32 (0.0f)) ? m_height : -m_height;
return dgVector (x0, dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f));
}
// dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f));
dgVector sideDir (m_yzMask & dir);
// sideDir = sideDir.Scale3 (m_radius * dgRsqrt (sideDir % sideDir + dgFloat32 (1.0e-18f)));
sideDir = sideDir.CompProduct4(sideDir.InvMagSqrt());
// return sideDir + dir.Scale3 (m_height);
return sideDir.Scale4(m_radius) + dir.Scale4 (m_height);
}
dgVector dgCollisionSphere::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgAssert (dir.m_w == 0.0f);
return dir.Scale4 (m_radius);
}
dgVector dgCollisionCone::SupportVertexSpecialProjectPoint(const dgVector& point, const dgVector& dir) const
{
dgAssert(dgAbsf((dir % dir - dgFloat32(1.0f))) < dgFloat32(1.0e-3f));
return point + dir.Scale4(D_CONE_SKIN_THINCKNESS);
}
dgVector dgCollisionBox::SupportVertexSpecialProjectPoint(const dgVector& point, const dgVector& dir) const
{
dgAssert(dgAbsf((dir.DotProduct3(dir) - dgFloat32(1.0f))) < dgFloat32(1.0e-3f));
return point + dir.Scale4 (D_BOX_SKIN_THINCKNESS);
}
dgVector dgCollisionChamferCylinder::ConvexConicSupporVertex (const dgVector& point, const dgVector& dir) const
{
dgAssert (dir.m_w == 0.0f);
return point + dir.Scale4(m_height);
}
dgInt32 dgCollisionBox::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgVector support[4];
dgInt32 featureCount = 3;
const dgConvexSimplexEdge** const vertToEdgeMapping = GetVertexToEdgeMapping();
if (vertToEdgeMapping) {
dgInt32 edgeIndex;
support[0] = SupportVertex (normal.Scale4(normalSign), &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_x > 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;
}
dgVector dgCollisionChamferCylinder::SupportVertexSpecialProjectPoint (const dgVector& point, const dgVector& dir) const
{
dgAssert (dir.m_w == 0.0f);
return point + dir.Scale4(m_height);
}
