//private static SceneData _data;
public static List <Vector4> Intersect(IIntersectable P, IIntersectable Q, SceneData scene, double newtonStep, out bool cycleIntersection)
{
if (P == null)
{
throw new ArgumentNullException(nameof(P));
}
if (Q == null)
{
throw new ArgumentNullException(nameof(Q));
}
double d = newtonStep;
cycleIntersection = false;
//_data = scene;
int number = 32;
var pPoints = P.GetPointsForSearch(number, number).ToList();
var qPoints = Q.GetPointsForSearch(number, number).ToList();
var closestDistance = double.MaxValue;
var pPoint = new ParametrizedPoint();
var qPoint = new ParametrizedPoint();
if (scene.Cursor.Visible)
{
var cursorPos = scene.Cursor.Transform.Position;
foreach (var pP in pPoints)
{
if ((pP.Position - cursorPos).LengthSquared() < closestDistance)
{
closestDistance = (pP.Position - cursorPos).LengthSquared();
pPoint = pP;
}
}
closestDistance = double.MaxValue;
foreach (var qP in qPoints)
{
if ((qP.Position - cursorPos).LengthSquared() < closestDistance)
{
closestDistance = (qP.Position - cursorPos).LengthSquared();
qPoint = qP;
}
}
}
else
{
foreach (var pP in pPoints)
{
foreach (var qP in qPoints)
{
if ((qP.Position - pP.Position).LengthSquared() < closestDistance)
{
closestDistance = (qP.Position - pP.Position).LengthSquared();
pPoint = pP;
qPoint = qP;
}
}
}
}
Func <Vector4, double> distanceFun = arg => (P.GetPosition(arg.X, arg.Y) - Q.GetPosition(arg.Z, arg.W)).LengthSquared();
Func <Vector4, Vector4> distanceGradient = arg =>
{
var diff = P.GetPosition(arg.X, arg.Y) - Q.GetPosition(arg.Z, arg.W);
var du = P.GetFirstParamDerivative(arg.X, arg.Y);
var dv = P.GetSecondParamDerivative(arg.X, arg.Y);
var ds = Q.GetFirstParamDerivative(arg.Z, arg.W);
var dt = Q.GetSecondParamDerivative(arg.Z, arg.W);
return(new Vector4(Vector3.DotProduct(diff, du), Vector3.DotProduct(diff, dv), -Vector3.DotProduct(diff, ds), -Vector3.DotProduct(diff, dt)) * 2.0);
};
var startPoint = SimpleGradient(scene, distanceFun, distanceGradient, pPoint, qPoint, P, Q);
if (startPoint == null)
{
return(null);
}
scene.CreateHiddenCatPoint(P.GetPosition(startPoint.Value.X, startPoint.Value.Y));
Func <Vector4, bool, Tuple <Matrix4, Vector3> > jacobian = (arg, invert) =>
{
var du = P.GetFirstParamDerivative(arg.X, arg.Y);
var dv = P.GetSecondParamDerivative(arg.X, arg.Y);
var ds = Q.GetFirstParamDerivative(arg.Z, arg.W);
var dt = Q.GetSecondParamDerivative(arg.Z, arg.W);
Matrix4 jacob = new Matrix4
{
