dgInt32 dgCollisionSphere::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgAssert (normal.m_w == 0.0f);
//contactsOut[0] = normal.Scale3(normal % point);
contactsOut[0] = normal.CompProduct4 (normal.DotProduct4(point));
return 1;
}
dgVector dgCollisionCone::SupportVertexSpecial(const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert(dgAbsf((dir % dir - dgFloat32(1.0f))) < dgFloat32(1.0e-3f));
if (dir.m_x < dgFloat32(-0.9999f)) {
return dgVector(-(m_height - D_CONE_SKIN_THINCKNESS), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else if (dir.m_x > dgFloat32(0.9999f)) {
return dgVector(m_height - D_CONE_SKIN_THINCKNESS, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else {
dgVector dir_yz(dir);
dir_yz.m_x = dgFloat32(0.0f);
dgFloat32 mag2 = dir_yz.DotProduct4(dir_yz).GetScalar();
dgAssert(mag2 > dgFloat32(0.0f));
dir_yz = dir_yz.Scale4(dgFloat32(1.0f) / dgSqrt(mag2));
dgVector p0(dir_yz.Scale4(m_radius - D_CONE_SKIN_THINCKNESS));
dgVector p1(dgVector::m_zero);
p0.m_x = -(m_height - D_CONE_SKIN_THINCKNESS);
p1.m_x = m_height - D_CONE_SKIN_THINCKNESS;
dgFloat32 dist0 = dir.DotProduct4(p0).GetScalar();
dgFloat32 dist1 = dir.DotProduct4(p1).GetScalar();
if (dist1 >= dist0) {
p0 = p1;
}
return p0;
}
}
dgVector dgCollisionCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));
if (dir.m_x < dgFloat32(-0.9999f)) {
return dgVector(-m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else if (dir.m_x > dgFloat32(0.9999f)) {
return dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else {
dgVector dir_yz (dir);
dir_yz.m_x = dgFloat32 (0.0f);
dgFloat32 mag2 = dir_yz.DotProduct4(dir_yz).GetScalar();
dgAssert (mag2 > dgFloat32 (0.0f));
dir_yz = dir_yz.Scale4 (dgFloat32 (1.0f) / dgSqrt (mag2));
dgVector p0 (dir_yz.Scale4 (m_radio0));
dgVector p1 (dir_yz.Scale4 (m_radio1));
p0.m_x = -m_height;
p1.m_x = m_height;
dgFloat32 dist0 = dir.DotProduct4(p0).GetScalar();
dgFloat32 dist1 = dir.DotProduct4(p1).GetScalar();
if (dist1 >= dist0) {
p0 = p1;
}
return p0;
}
}
dgInt32 dgCollisionSphere::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgAssert (normal.m_w == 0.0f);
dgAssert ((normal % normal) > dgFloat32 (0.999f));
//contactsOut[0] = normal.Scale3 (normal % point);
contactsOut[0] = normal.CompProduct4 (normal.DotProduct4(point));
return 1;
}
dgInt32 dgCollisionChamferCylinder::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
dgFloat32 disc2 = point.m_y * point.m_y + point.m_z * point.m_z;
if (disc2 < m_radius * m_radius) {
dgVector cylinderNormal ((point.m_x >= dgFloat32 (0.0)) ? dgFloat32 (-1.0f) : dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
return CalculateContactsGeneric (point, cylinderNormal, proxy, contactsOut);
} else {
dgVector r (dgFloat32 (0.0f), point.m_y, point.m_z, dgFloat32 (0.0f));
dgAssert ((r % r) > dgFloat32 (0.0f));
// r = r.Scale3(m_radius * dgRsqrt (r % r));
r = r.CompProduct4(r.InvMagSqrt()).Scale4(m_radius);
//dgFloat32 t = normal % (r - point);
dgVector t (normal.DotProduct4(r - point));
//contactsOut[0] = r - normal.Scale3 (t);
contactsOut[0] = r - normal.CompProduct4(t);
return 1;
}
}
void dgCollisionConvexPolygon::BeamClipping (const dgVector& origin, dgFloat32 dist)
{
dgPlane planes[4];
dgVector points[sizeof (m_localPoly) / sizeof (m_localPoly[0]) + 8];
dgClippedFaceEdge clippedFace [2 * sizeof (m_localPoly) / sizeof (m_localPoly[0]) + 8];
dgVector dir (m_localPoly[1] - m_localPoly[0]);
dgAssert (dir.m_w == dgFloat32 (0.0f));
dgAssert ((dir % dir) > dgFloat32 (1.0e-8f));
dir = dir.CompProduct4 (dir.InvMagSqrt());
dgFloat32 distH = origin.DotProduct4(dir).GetScalar();
planes[0] = dgPlane (dir, dist - distH);
planes[2] = dgPlane (dir.CompProduct4 (dgVector::m_negOne), dist + distH);
dir = m_normal * dir;
dgFloat32 distV = origin.DotProduct4(dir).GetScalar();
planes[1] = dgPlane (dir, dist - distV);
planes[3] = dgPlane (dir.CompProduct4 (dgVector::m_negOne), dist + distV);
for (dgInt32 i = 0; i < m_count; i ++) {
dgInt32 j = i << 1;
dgAssert (j < sizeof (clippedFace) / sizeof (clippedFace[0]));
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_adjacentFaceEdgeNormalIndex[i];
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 + 1].m_incidentNormal = -1;
}
clippedFace[1].m_next = &clippedFace[m_count * 2 - 2 + 1];
dgAssert ((m_count * 2 - 2) >= 0);
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;
dgClippedFaceEdge* first = &clippedFace[0];
for (dgInt32 i = 0; i < 4; i ++) {
const dgPlane& plane = planes[i];
dgInt32 conectCount = 0;
dgClippedFaceEdge* connect[2];
dgClippedFaceEdge* ptr = first;
dgClippedFaceEdge* newFirst = first;
dgFloat32 test0 = plane.Evalue(points[ptr->m_incidentVertex]);
do {
dgFloat32 test1 = plane.Evalue(points[ptr->m_next->m_incidentVertex]);
if (test0 > dgFloat32 (1.0e-2f)) {
if (test1 <= dgFloat32 (-1.0e-2f)) {
const dgVector& p0 = points[ptr->m_incidentVertex];
const dgVector& p1 = points[ptr->m_next->m_incidentVertex];
dgVector dp (p1 - p0);
points[indexCount] = p0 - dp.Scale4 (test0 / dp.DotProduct4(plane).GetScalar());
dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_twin->m_incidentNormal = ptr->m_incidentNormal;
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 (1.0e-2f)) {
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.Scale4 (test0 / dp.DotProduct4(plane).GetScalar());
dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_twin->m_incidentNormal = ptr->m_incidentNormal;;
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 > 1) {
first = newFirst;
dgAssert (conectCount == 2);
dgClippedFaceEdge* const newEdge = &clippedFace[edgeCount];
newEdge->m_twin = newEdge + 1;
newEdge->m_twin->m_twin = newEdge;
newEdge->m_incidentNormal = m_faceNormalIndex;;
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_incidentNormal = m_faceNormalIndex;;
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;
}
}
dgClippedFaceEdge* ptr = first;
do {
dgVector dist (points[ptr->m_next->m_incidentVertex] - points[ptr->m_incidentVertex]);
dgFloat32 error = dist % dist;
if (error < dgFloat32 (1.0e-6f)) {
ptr->m_next = ptr->m_next->m_next;
first = ptr;
}
ptr = ptr->m_next;
} while (ptr != first);
dgInt32 count = 0;
dgInt32 isConvexCap = 0;
m_adjacentFaceEdgeNormalIndex = &m_clippEdgeNormal[0];
do {
m_clippEdgeNormal[count] = ptr->m_incidentNormal;
isConvexCap |= (ptr->m_incidentNormal - m_faceNormalIndex);
m_localPoly[count] = points[ptr->m_incidentVertex];
count ++;
ptr = ptr->m_next;
} while (ptr != first);
m_count = count;
}
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;
}
