private Vector3D EvaluateIntersectionFunction(
IntersectionParametrisation parametrisation
)
{
return(_surfaces[0].Evaluate(parametrisation.First) -
_surfaces[1].Evaluate(parametrisation.Second));
}
private DenseMatrix GetJacobian(
IntersectionParametrisation parametrisation
)
{
var dfU = _surfaces[0].Derivative(
parametrisation.First,
DerivativeParameter.U
);
var dfV = _surfaces[0].Derivative(
parametrisation.First,
DerivativeParameter.V
);
var dgU = -1.0 * _surfaces[1].Derivative(
parametrisation.Second,
DerivativeParameter.U
);
var dgV = -1.0 * _surfaces[1].Derivative(
parametrisation.Second,
DerivativeParameter.V
);
return(DenseMatrix.OfArray(new[, ]
{
{ dfU.X, dfV.X, dgU.X, dgV.X },
{ dfU.Y, dfV.Y, dgU.Y, dgV.Y },
{ dfU.Z, dfV.Z, dgU.Z, dgV.Z },
}));
}
private DenseMatrix GetPlaneJacobian(
IntersectionParametrisation previousParametrisation,
Vector previousPoint,
Vector tangentVector
)
{
var planeUDerivative = EvaluateNumericalDerivativeForPlaneEquation(
_surfaces[0],
previousParametrisation.First,
previousPoint,
tangentVector,
DerivativeParameter.U
);
var planeVDerivative = EvaluateNumericalDerivativeForPlaneEquation(
_surfaces[0],
previousParametrisation.First,
previousPoint,
tangentVector,
DerivativeParameter.V
);
var planeJacobian = DenseMatrix.OfArray(
new[, ]
{
{ planeUDerivative[0], planeVDerivative[0], 0.0, 0.0 },
{ planeUDerivative[1], planeVDerivative[1], 0.0, 0.0 },
{ planeUDerivative[2], planeVDerivative[2], 0.0, 0.0 },
}
);
return(planeJacobian);
}
private int FindParametrisationIndex(
IntersectionParametrisation parametrisation
)
{
for (var i = 0; i < Polygon.Count - 10; ++i)
{
var norm1 = Parametrisation.DistanceNormMax(
parametrisation.First,
Polygon[i].First
);
var norm2 = Parametrisation.DistanceNormMax(
parametrisation.Second,
Polygon[i].Second
);
//IntersectionParametrisation.DistanceNormMax(
//parametrisation,
//Polygon[i]
//);
if (norm1 < EqualityEpsilon || norm2 < EqualityEpsilon)
{
return(i);
}
}
return(-1);
}
private bool AreEdgeTrackingBoundaryConditionsFullfilled(
IntersectionParametrisation?previousParametrisation,
IntersectionParametrisation?currentIntersectionParametrisation
)
{
if (!(currentIntersectionParametrisation?.IsValid() ?? false))
{
return(true);
}
if (!previousParametrisation.HasValue)
{
return(false);
}
Debug.Assert(
currentIntersectionParametrisation != null,
"currentIntersectionParametrisation != null"
);
if (MinimumStepLength >=
IntersectionParametrisation.DistanceNormMax(
previousParametrisation.Value,
currentIntersectionParametrisation.Value
))
{
return(true);
}
return(false);
}
public static double DistanceNormMax(
IntersectionParametrisation left,
IntersectionParametrisation right
)
{
return(Math.Max(
Parametrisation.DistanceNormMax(left.First, right.First),
Parametrisation.DistanceNormMax(left.Second, right.Second)
));
}
private Vector3D EvaluateTangent(
IntersectionParametrisation previousParametrisation
)
{
var normal1 = _surfaces[0].Normal(previousParametrisation.First);
var normal2 = _surfaces[1].Normal(previousParametrisation.Second);
var tangent = Vector3D.CrossProduct(normal1, normal2);
tangent.Normalize();
return(tangent);
}
private static IntersectionParametrisation EvaluateNewtonStep(
IntersectionParametrisation previousParametrisation,
Matrix jacobian,
Vector evaluatedFunction
)
{
var jacobianInv = GetPseudoInverse(jacobian);
var correction = jacobianInv * evaluatedFunction;
return(previousParametrisation - correction);
}
private Matrix GetJacobianWithPlane(
IntersectionParametrisation previousParametrisation,
Vector previousPoint,
Vector tangentVector
)
{
var simpleJacobian = GetJacobian(previousParametrisation);
var planeJacobian = GetPlaneJacobian(
previousParametrisation,
previousPoint,
tangentVector
);
return(simpleJacobian - planeJacobian);
}
private IntersectionParametrisation EvaluateTwoSurfacesNewtonStep(
IntersectionParametrisation previousParametrisation
)
{
var evaluatedFunction =
EvaluateIntersectionFunction(previousParametrisation)
.ToMathVector();
var jacobian = GetJacobian(previousParametrisation);
return(EvaluateNewtonStep(
previousParametrisation,
jacobian,
evaluatedFunction
));
}
private Vector EvaluateIntersectionWithPlaneFunction(
IntersectionParametrisation parametrisation,
Vector previousPoint,
Vector tangentVector
)
{
var tantentPlaneEquationVector = EvaluatePlaneEquation(
_surfaces[0],
parametrisation.First,
previousPoint,
tangentVector
);
return(EvaluateIntersectionFunction(parametrisation).ToMathVector()
- tantentPlaneEquationVector);
}
public bool FinishIfLoopedBack(
IntersectionParametrisation parametrisation
)
{
var index = FindParametrisationIndex(parametrisation);
if (index == -1)
{
return(false);
}
Polygon = Polygon.Skip(index).ToList();
Polygon.Add(parametrisation);
Polygon.Add(Polygon.First());
IsLooped = true;
return(true);
}
private IntersectionParametrisation?GetNextInitialGuess(
IntersectionParametrisation previousGuess
)
{
var previousPoint = ((Vector3D)_surfaces[0].Evaluate(
previousGuess.First
)).ToMathVector();
var tangent = EvaluateTangent(previousGuess).ToMathVector();
var iterationNumber = 0;
var currentParametrisation = previousGuess;
do
{
var nextParametrisation = EvaluateNextGuessNewtonStep(
currentParametrisation,
previousPoint,
tangent
);
if (!nextParametrisation.IsValid())
{
return(null);
}
if (MinimumStepLength >=
IntersectionParametrisation.DistanceNormMax(
nextParametrisation,
currentParametrisation
))
{
return(nextParametrisation);
}
currentParametrisation = nextParametrisation;
} while (++iterationNumber < MaximumNewtonIterations);
return(null);
}
private IntersectionParametrisation?FindFirstIntersection(
Parametrisation startingParametersFirstSurface,
Parametrisation startingParametersSecondSurface
)
{
var currentParametrisation = new IntersectionParametrisation(
startingParametersFirstSurface,
startingParametersSecondSurface
);
var iterationNumber = 0;
do
{
var nextParametrisation = EvaluateTwoSurfacesNewtonStep(
currentParametrisation
);
if (!nextParametrisation.IsValid())
{
return(null);
}
if (MinimumStepLength >=
IntersectionParametrisation.DistanceNormMax(
nextParametrisation,
currentParametrisation
))
{
return(nextParametrisation);
}
currentParametrisation = nextParametrisation;
} while (++iterationNumber < MaximumNewtonIterations);
return(null);
}
private IntersectionParametrisation EvaluateNextGuessNewtonStep(
IntersectionParametrisation previousParametrisation,
Vector previousPoint,
Vector tangent
)
{
var evaluated = EvaluateIntersectionWithPlaneFunction(
previousParametrisation,
previousPoint,
tangent
);
var jacobian = GetJacobianWithPlane(
previousParametrisation,
previousPoint,
tangent
);
return(EvaluateNewtonStep(
previousParametrisation,
jacobian,
evaluated
));
}
public void FindLoopAndIsolate(IntersectionParametrisation value)
{
}
public void Add(IntersectionParametrisation parametrisation)
{
Polygon.Add(parametrisation);
}