/// Changes a btManifoldPoint collision normal to the normal from the mesh.
public static void AdjustInternalEdgeContacts(ManifoldPoint cp, CollisionObject colObj0, CollisionObject colObj1, int partId0, int index0, InternalEdgeAdjustFlags normalAdjustFlags)
{
//btAssert(colObj0.GetCollisionShape().GetShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
if (colObj0.GetCollisionShape().GetShapeType() != BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
{
return;
}
BvhTriangleMeshShape trimesh = null;
if (colObj0.GetRootCollisionShape().GetShapeType() == BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//trimesh = ((ScaledBvhTriangleMeshShape)colObj0.GetRootCollisionShape()).GetChildShape();
}
else
{
trimesh = (BvhTriangleMeshShape)colObj0.GetRootCollisionShape();
}
TriangleInfoMap triangleInfoMapPtr = (TriangleInfoMap)trimesh.GetTriangleInfoMap();
if (triangleInfoMapPtr == null)
{
return;
}
int hash = GetHash(partId0, index0);
TriangleInfo info;
if (!triangleInfoMapPtr.TryGetValue(hash, out info))
{
return;
}
float frontFacing = (normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.0f : -1.0f;
TriangleShape tri_shape = colObj0.GetCollisionShape() as TriangleShape;
IndexedVector3 v0, v1, v2;
tri_shape.GetVertex(0, out v0);
tri_shape.GetVertex(1, out v1);
tri_shape.GetVertex(2, out v2);
IndexedVector3 center = (v0 + v1 + v2) * (1.0f / 3.0f);
IndexedVector3 red = new IndexedVector3(1, 0, 0), green = new IndexedVector3(0, 1, 0), blue = new IndexedVector3(0, 0, 1), white = new IndexedVector3(1, 1, 1), black = new IndexedVector3(0, 0, 0);
IndexedVector3 tri_normal;
tri_shape.CalcNormal(out tri_normal);
//float dot = tri_normal.dot(cp.m_normalWorldOnB);
IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
IndexedVector3 contact = cp.m_localPointB;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedMatrix tr = colObj0.GetWorldTransform();
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
bool isNearEdge = false;
int numConcaveEdgeHits = 0;
int numConvexEdgeHits = 0;
IndexedVector3 localContactNormalOnB = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
localContactNormalOnB.Normalize();//is this necessary?
// Get closest edge
int bestedge = -1;
float disttobestedge = MathUtil.BT_LARGE_FLOAT;
//
// Edge 0 . 1
if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 0;
disttobestedge = len;
}
}
// Edge 1 . 2
if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v1, ref v2, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 1;
disttobestedge = len;
}
}
// Edge 2 . 0
if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v2, ref v0, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 2;
disttobestedge = len;
}
}
#if BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 upfix = tri_normal * new IndexedVector3(0.1f, 0.1f, 0.1f);
DebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
#endif
if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
{
if (bestedge == 0)
{
IndexedVector3 edge = (v0 - v1);
isNearEdge = true;
if (info.m_edgeV0V1Angle == 0.0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV0V1Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info.m_edgeV0V1Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal. (what about cp.m_distance1?)
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
}
NearestPointInLineSegment(ref contact, ref v1, ref v2, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
#endif
if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
{
if (bestedge == 1)
{
isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 edge = (v1 - v2);
isNearEdge = true;
if (info.m_edgeV1V2Angle == 0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV1V2Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV1V2Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
}
NearestPointInLineSegment(ref contact, ref v2, ref v0, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
#endif
if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
{
if (bestedge == 2)
{
isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 edge = (v2 - v0);
if (info.m_edgeV2V0Angle == 0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV2V0Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
// printf("hitting convex edge\n");
IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV2V0Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis *clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
}
#if DEBUG_INTERNAL_EDGE
{
IndexedVector3 color = new IndexedVector3(0, 1, 1);
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
}
#endif //DEBUG_INTERNAL_EDGE
if (isNearEdge)
{
if (numConcaveEdgeHits > 0)
{
if ((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0)
{
//fix tri_normal so it pointing the same direction as the current local contact normal
if (tri_normal.Dot(ref localContactNormalOnB) < 0)
{
tri_normal *= -1;
}
cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis *tri_normal;
}
else
{
IndexedVector3 newNormal = tri_normal * frontFacing;
//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
float d = newNormal.Dot(ref localContactNormalOnB);
if (d < 0)
{
return;
}
//modify the normal to be the triangle normal (or backfacing normal)
cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis *newNormal;
}
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
/// Changes a btManifoldPoint collision normal to the normal from the mesh.
public static void AdjustInternalEdgeContacts(ManifoldPoint cp, CollisionObject colObj0, CollisionObject colObj1, int partId0, int index0, InternalEdgeAdjustFlags normalAdjustFlags)
{
//btAssert(colObj0.GetCollisionShape().GetShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
if (colObj0.GetCollisionShape().GetShapeType() != BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE)
return;
BvhTriangleMeshShape trimesh = null;
if (colObj0.GetRootCollisionShape().GetShapeType() == BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//trimesh = ((ScaledBvhTriangleMeshShape)colObj0.GetRootCollisionShape()).GetChildShape();
}
else
{
trimesh = (BvhTriangleMeshShape)colObj0.GetRootCollisionShape();
}
TriangleInfoMap triangleInfoMapPtr = (TriangleInfoMap)trimesh.GetTriangleInfoMap();
if (triangleInfoMapPtr == null)
{
return;
}
int hash = GetHash(partId0, index0);
TriangleInfo info;
if (!triangleInfoMapPtr.TryGetValue(hash, out info))
{
return;
}
float frontFacing = (normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.0f : -1.0f;
TriangleShape tri_shape = colObj0.GetCollisionShape() as TriangleShape;
IndexedVector3 v0, v1, v2;
tri_shape.GetVertex(0, out v0);
tri_shape.GetVertex(1, out v1);
tri_shape.GetVertex(2, out v2);
IndexedVector3 center = (v0 + v1 + v2) * (1.0f / 3.0f);
IndexedVector3 red = new IndexedVector3(1, 0, 0), green = new IndexedVector3(0, 1, 0), blue = new IndexedVector3(0, 0, 1), white = new IndexedVector3(1, 1, 1), black = new IndexedVector3(0, 0, 0);
IndexedVector3 tri_normal;
tri_shape.CalcNormal(out tri_normal);
//float dot = tri_normal.dot(cp.m_normalWorldOnB);
IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
IndexedVector3 contact = cp.m_localPointB;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedMatrix tr = colObj0.GetWorldTransform();
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
bool isNearEdge = false;
int numConcaveEdgeHits = 0;
int numConvexEdgeHits = 0;
IndexedVector3 localContactNormalOnB = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
localContactNormalOnB.Normalize();//is this necessary?
// Get closest edge
int bestedge = -1;
float disttobestedge = MathUtil.BT_LARGE_FLOAT;
//
// Edge 0 . 1
if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v0, ref v1, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 0;
disttobestedge = len;
}
}
// Edge 1 . 2
if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v1, ref v2, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 1;
disttobestedge = len;
}
}
// Edge 2 . 0
if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
//IndexedVector3 nearest;
NearestPointInLineSegment(ref cp.m_localPointB, ref v2, ref v0, out nearest);
float len = (contact - nearest).Length();
//
if (len < disttobestedge)
{
bestedge = 2;
disttobestedge = len;
}
}
#if BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 upfix = tri_normal * new IndexedVector3(0.1f, 0.1f, 0.1f);
DebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red);
#endif
if (Math.Abs(info.m_edgeV0V1Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
if (bestedge == 0)
{
IndexedVector3 edge = (v0 - v1);
isNearEdge = true;
if (info.m_edgeV0V1Angle == 0.0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV0V1Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info.m_edgeV0V1Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal. (what about cp.m_distance1?)
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
NearestPointInLineSegment(ref contact, ref v1, ref v2, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green);
#endif
if (Math.Abs(info.m_edgeV1V2Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
if (bestedge == 1)
{
isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 edge = (v1 - v2);
isNearEdge = true;
if (info.m_edgeV1V2Angle == 0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV1V2Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV1V2Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
NearestPointInLineSegment(ref contact, ref v2, ref v0, out nearest);
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue);
#endif
if (Math.Abs(info.m_edgeV2V0Angle) < triangleInfoMapPtr.m_maxEdgeAngleThreshold)
{
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
float len = (contact - nearest).Length();
if (len < triangleInfoMapPtr.m_edgeDistanceThreshold)
if (bestedge == 2)
{
isNearEdge = true;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 edge = (v2 - v0);
if (info.m_edgeV2V0Angle == 0f)
{
numConcaveEdgeHits++;
}
else
{
bool isEdgeConvex = (info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_CONVEX) != 0;
float swapFactor = isEdgeConvex ? 1.0f : -1.0f;
#if BT_INTERNAL_EDGE_DEBUG_DRAW
DebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
IndexedVector3 nA = swapFactor * tri_normal;
IndexedQuaternion orn = new IndexedQuaternion(edge, info.m_edgeV2V0Angle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(ref orn, ref tri_normal);
if ((info.m_flags & TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB) != 0)
{
computedNormalB *= -1;
}
IndexedVector3 nB = swapFactor * computedNormalB;
#if DEBUG_INTERNAL_EDGE
{
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + tr._basis * (nB * 20), red);
}
#endif //DEBUG_INTERNAL_EDGE
float NdotA = localContactNormalOnB.Dot(ref nA);
float NdotB = localContactNormalOnB.Dot(ref nB);
bool backFacingNormal = (NdotA < triangleInfoMapPtr.m_convexEpsilon) && (NdotB < triangleInfoMapPtr.m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
// printf("hitting convex edge\n");
IndexedVector3 localContactNormalOnB2 = colObj0.GetWorldTransform()._basis.Transpose() * cp.m_normalWorldOnB;
IndexedVector3 clampedLocalNormal;
bool isClamped = ClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB2, info.m_edgeV2V0Angle, out clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.Dot(frontFacing * tri_normal) > 0))
{
IndexedVector3 newNormal = colObj0.GetWorldTransform()._basis * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
#if DEBUG_INTERNAL_EDGE
{
IndexedVector3 color = new IndexedVector3(0, 1, 1);
DebugDrawLine(cp.GetPositionWorldOnB(), cp.GetPositionWorldOnB() + cp.m_normalWorldOnB * 10, color);
}
#endif //DEBUG_INTERNAL_EDGE
if (isNearEdge)
{
if (numConcaveEdgeHits > 0)
{
if ((normalAdjustFlags & InternalEdgeAdjustFlags.BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0)
{
//fix tri_normal so it pointing the same direction as the current local contact normal
if (tri_normal.Dot(ref localContactNormalOnB) < 0)
{
tri_normal *= -1;
}
cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis * tri_normal;
}
else
{
IndexedVector3 newNormal = tri_normal * frontFacing;
//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
float d = newNormal.Dot(ref localContactNormalOnB);
if (d < 0)
{
return;
}
//modify the normal to be the triangle normal (or backfacing normal)
cp.m_normalWorldOnB = colObj0.GetWorldTransform()._basis * newNormal;
}
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0.GetWorldTransform().InvXform(cp.m_positionWorldOnB);
}
}
}
