//--------------------------------------------------------------------------------------------------
/// Transforms the plane with the given homogeneous transformation \a matrix
//--------------------------------------------------------------------------------------------------
void Plane::transform(const Mat4d& matrix)
{
Vec3d n = normal();
Vec3d point = pointInPlane();
n.transformVector(matrix);
point.transformPoint(matrix);
setFromPointAndNormal(point, n);
}
//--------------------------------------------------------------------------------------------------
/// Project the given point onto the plane
//--------------------------------------------------------------------------------------------------
Vec3d Plane::projectPoint(const Vec3d& point) const
{
Vec3d pip = pointInPlane();
// Create vector from point in plane to node
Vec3d vector = point - pip;
// Project vector to find node in plane
Vec3d projectedPoint;
if (projectVector(vector, &projectedPoint))
{
projectedPoint += pip;
}
else
{
// The <point - pip> vector is parallel with the normal vector, use the point in plane as the projected
projectedPoint = pip;
}
return projectedPoint;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI));
dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI));
const dgCollisionInstance* const hull = proxy.m_referenceCollision;
dgAssert (this == proxy.m_floatingCollision->GetChildShape());
dgAssert (m_count);
dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const floatingBody = proxy.m_floatingBody;
const dgBody* const referenceBody = proxy.m_referenceBody;
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32 (1.0e10f);
m_normal = m_normal.CompProduct4(polyInstanceInvScale);
dgAssert (m_normal.m_w == dgFloat32 (0.0f));
m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
const dgVector savedFaceNormal (m_normal);
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride]));
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
}
dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f))));
hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne;
dgMatrix polygonMatrix;
polygonMatrix[0] = m_localPoly[1] - m_localPoly[0];
polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = hullOrigin;
dgAssert (polygonMatrix.TestOrthogonal());
dgMatrix savedProxyMatrix (proxy.m_matrix);
proxy.m_matrix = polygonMatrix * proxy.m_matrix;
dgVector floatingVeloc (floatingBody->m_veloc);
dgVector referenceVeloc (referenceBody->m_veloc);
const dgMatrix& hullMatrix = hull->GetGlobalMatrix();
dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc));
dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc));
dgVector polyBoxP0 (dgFloat32 ( 1.0e15f));
dgVector polyBoxP1 (dgFloat32 (-1.0e15f));
m_normal = polygonMatrix.UnrotateVector(m_normal);
if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]);
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]);
polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]);
}
dgInt32 count = 0;
dgVector hullBoxP0;
dgVector hullBoxP1;
hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1);
dgVector minBox (polyBoxP0 - hullBoxP1);
dgVector maxBox (polyBoxP1 - hullBoxP0);
dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
if (distance < dgFloat32 (1.0f)) {
dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// boxOrigin += polyRelativeVeloc.Scale4 (distance);
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal);
dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane));
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector e (m_localPoly[i] - m_localPoly[i0]);
dgVector n (m_normal * e);
dgPlane plane (n, - (m_localPoly[i0] % n));
dgVector supportDist (plane.Abs().DotProduct4 (boxSize));
dgFloat32 centerDist = plane.Evalue(boxOrigin);
if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
if ((centerDist - supportDist.m_x) < dgFloat32 (0.0f)) {
inside = false;
}
i0 = i;
}
// for the time being for the minkousky contact calculation
inside = false;
const dgInt32 hullId = hull->GetUserDataID();
if (inside) {
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector p0 (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 timetoImpact = dgFloat32 (0.0f);
//dgFloat32 closestDistance = dgFloat32 (0.0f);
dgAssert (0);
// dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness + DG_IMPULSIVE_CONTACT_PENETRATION;
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32 (0.0f)) {
timetoImpact = penetration / (polyRelativeVeloc % m_normal);
dgAssert (timetoImpact >= dgFloat32 (0.0f));
// closestDistance = -penetration;
}
if (timetoImpact <= proxy.m_timestep) {
dgVector pointsContacts[64];
contactJoint->m_closestDistance = penetration;
dgAssert (0);
// dgVector point (pointInHull - normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector point (pointInHull);
count = hull->CalculatePlaneIntersection (normalInHull, point, pointsContacts, 1.0f);
dgAssert (0);
// dgVector step (hullRelativeVeloc.Scale3 (timetoImpact) + normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector step (hullRelativeVeloc.Scale3 (timetoImpact));
penetration = dgMax (penetration, dgFloat32 (0.0f));
const dgMatrix& worldMatrix = hull->m_globalMatrix;
dgContactPoint* const contactsOut = proxy.m_contacts;
dgVector globalNormal (worldMatrix.RotateVector(normalInHull));
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_point = worldMatrix.TransformVector (pointsContacts[i] + step);
contactsOut[i].m_normal = globalNormal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
} else {
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize ();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping (boxOrigin, convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
dgCollisionConvex* const convexShape = (dgCollisionConvex*) hull->m_childShape;
count = convexShape->CalculateConvexCastContacts (proxy);
// dgAssert (proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
#if 0
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal (polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
if ((normal % savedFaceNormal) < dgFloat32 (0.995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector n (&m_vertex[index * m_stride]);
dgVector dir0 (n * savedFaceNormal);
dgVector dir1 (n * normal);
dgFloat32 projection = dir0 % dir1;
if (projection <= dgFloat32 (0.0f)) {
normal = n;
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_normal = normal;
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i ++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
#endif
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
proxy.m_matrix = savedProxyMatrix;
return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert(this == proxy.m_instance1->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const body0 = proxy.m_body0;
const dgBody* const body1 = proxy.m_body1;
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
dgVector relativeVelocity (body0->m_veloc - body1->m_veloc);
if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) {
return 0;
}
dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal);
if (den > dgFloat32 (1.0e-5f)) {
// this can actually happens
dgAssert(0);
return 0;
}
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32(1.0e10f);
dgMatrix polygonMatrix;
dgVector right (m_localPoly[1] - m_localPoly[0]);
polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = dgVector::m_wOne;
dgAssert (polygonMatrix.TestOrthogonal());
dgVector polyBoxP0(dgFloat32(1.0e15f));
dgVector polyBoxP1(dgFloat32(-1.0e15f));
for (dgInt32 i = 0; i < m_count; i++) {
dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i]));
polyBoxP0 = polyBoxP0.GetMin(point);
polyBoxP1 = polyBoxP1.GetMax(point);
}
dgVector hullBoxP0;
dgVector hullBoxP1;
dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix);
proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1);
dgVector minBox(polyBoxP0 - hullBoxP1);
dgVector maxBox(polyBoxP1 - hullBoxP0);
dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity));
dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
dgInt32 count = 0;
if (distance < dgFloat32(1.0f)) {
bool inside = false;
dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half));
dgVector sphereMag2 (boxSize.DotProduct4(boxSize));
boxSize = sphereMag2.Sqrt();
dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den;
dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den;
dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0));
dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1));
dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half));
dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half)));
sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize);
boxSize = sphereMag2.Sqrt();
dgAssert (boxOrigin.m_w == 0.0f);
boxOrigin = boxOrigin | dgVector::m_wOne;
if (!proxy.m_intersectionTestOnly) {
inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e & dgVector::m_triplexMask);
dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar());
dgPlane plane(n, -(m_localPoly[i0] % n));
dgVector p0 (boxOrigin + n.Scale4 (param));
dgVector p1 (boxOrigin - n.Scale4 (param));
dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar();
dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar();
if ((size0 < 0.0f) && (size1 < 0.0f)) {
return 0;
}
if ((size0 * size1) < 0.0f) {
inside = false;
break;
}
i0 = i;
}
}
dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar());
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(boxOrigin, convexSphapeUmbra);
m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = proxy.m_instance0->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix;
dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f))));
dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL));
dgVector p0 (matrixInstance0.TransformVector(pointInHull));
dgFloat32 timetoImpact = dgFloat32(0.0f);
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
timetoImpact = penetration / (relativeVelocity % m_normal);
dgAssert(timetoImpact >= dgFloat32(0.0f));
}
if (timetoImpact <= proxy.m_timestep) {
dgVector contactPoints[64];
contactJoint->m_closestDistance = penetration;
proxy.m_timestep = timetoImpact;
proxy.m_normal = m_normal;
proxy.m_closestPointBody0 = p0;
proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration);
if (!proxy.m_intersectionTestOnly) {
pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP);
count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints);
dgVector step(relativeVelocity.Scale4(timetoImpact));
penetration = dgMax(penetration, dgFloat32(0.0f));
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step;
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
return count;
}
