public virtual bool sphere_intersects_poly(Sphere checkPos, Vector3 movement, ref Polygon polygon, ref Vector3 contactPoint)
{
if (!Sphere.Intersects(checkPos))
{
return(false);
}
var dist = Vector3.Dot(SplittingPlane.Normal, checkPos.Center) + SplittingPlane.D;
var reach = checkPos.Radius - PhysicsGlobals.EPSILON;
if (dist >= reach)
{
return(PosNode.sphere_intersects_poly(checkPos, movement, ref polygon, ref contactPoint));
}
if (dist <= -reach)
{
return(NegNode.sphere_intersects_poly(checkPos, movement, ref polygon, ref contactPoint));
}
if (PosNode != null && PosNode.sphere_intersects_poly(checkPos, movement, ref polygon, ref contactPoint))
{
return(true);
}
if (NegNode != null && NegNode.sphere_intersects_poly(checkPos, movement, ref polygon, ref contactPoint))
{
return(true);
}
return(false);
}
public bool adjust_to_plane(Sphere checkPos, Vector3 curPos, Polygon hitPoly, Vector3 contactPoint)
{
var movement = checkPos.Center - curPos;
var lowerTime = 0.0;
var upperTime = 1.0;
var maxIterations = 15; // hardcoded
for (var i = 0; i < maxIterations; i++)
{
var touchTime = hitPoly.adjust_sphere_to_poly(checkPos, curPos, movement);
if (touchTime == 1.0f)
{
checkPos.Center = curPos + movement * (float)touchTime;
if (!RootNode.sphere_intersects_poly(checkPos, movement, ref hitPoly, ref contactPoint))
{
lowerTime = touchTime;
break;
}
upperTime = touchTime;
}
if (i == maxIterations - 1)
{
return(false);
}
}
for (var j = 0; j < maxIterations; j++)
{
var averageTime = (lowerTime + upperTime) * 0.5f;
checkPos.Center = curPos + movement * (float)averageTime;
if (!RootNode.sphere_intersects_poly(checkPos, movement, ref hitPoly, ref contactPoint))
{
upperTime = (lowerTime + upperTime) * 0.5f;
}
else
{
lowerTime = (lowerTime + upperTime) * 0.5f;
}
if (upperTime - lowerTime < 0.02)
{
return(false);
}
}
return(true);
}
