public static MathPoint Project(this IMathPoint point, IMathPoint origin, IMathVector axis)
{
var a = (IMathVector) point.Subtract(origin);
var t = a.Project(axis);
var v = (MathVector) axis.Scale(t);
return (MathPoint) origin.AddVector(v);
}
public static MathVector Project(this ITriadManipulator m, swTriadManipulatorControlPoints_e h, IMathPoint p, IMathVector cameraVector, IMathUtility mathP)
{
IMathUtility math = mathP;
var zero = (IMathPoint) math.CreatePoint(new[] {0, 0, 0});
double pT, qT;
switch (h)
{
case swTriadManipulatorControlPoints_e.swTriadManipulatorOrigin:
return (MathVector) p.Subtract(m.Origin);
case swTriadManipulatorControlPoints_e.swTriadManipulatorXAxis:
return ClosestPointOnAxis(m, p, cameraVector, m.XAxis);
case swTriadManipulatorControlPoints_e.swTriadManipulatorYAxis:
return ClosestPointOnAxis(m, p, cameraVector, m.YAxis);
case swTriadManipulatorControlPoints_e.swTriadManipulatorZAxis:
return ClosestPointOnAxis(m, p, cameraVector, m.ZAxis);
case swTriadManipulatorControlPoints_e.swTriadManipulatorXYPlane:
return (MathVector) p.Subtract(m.Origin);
case swTriadManipulatorControlPoints_e.swTriadManipulatorYZPlane:
return (MathVector) p.Subtract(m.Origin);
case swTriadManipulatorControlPoints_e.swTriadManipulatorZXPlane:
return (MathVector) p.Subtract(m.Origin);
default:
throw new ArgumentOutOfRangeException(nameof(h), h, null);
}
}
public static double AngleBetweenVectors(this IMathVector v0, IMathVector v1)
{
if (((double[])v0.ArrayData).Length==((double[])v1.ArrayData).Length)
{
var sign = Math.Sign(((IMathVector)(v0.Cross(v1))).ArrayData.CastArray<double>()[2]);
var ret = Math.Acos(v0.Dot(v1) / (v0.GetLength() * v1.GetLength()));
return sign * ret;
}
throw new Exception("Vectors must have the same dimension!");
}
private static MathVector ClosestPointOnAxis(ITriadManipulator m, IMathPoint p, IMathVector cameraVector, MathVector axis)
{
double pT;
double qT;
if (ClosestPointBetweenLines(m.Origin, axis, p, cameraVector, out pT, out qT))
{
return (MathVector) axis.Scale(pT);
}
else
{
return null;
}
}
public static double GetAngle(this IMathVector vec1, IMathVector vec2)
{
return(Math.Acos(vec1.Dot(vec2) / (vec1.GetLength() * vec2.GetLength())));
}
public static MathVector SubtractTs(this IMathVector a, IMathVector b)
{
return((MathVector)a.Subtract(b));
}
public static MathVector ScaleTs(this IMathVector a, double b)
{
return((MathVector)a.Scale(b));
}
public static MathVector AlphaBend(this IMathVector a, IMathVector b, double alpha)
{
Debug.Assert(alpha>=0);
Debug.Assert(alpha<=1);
return (MathVector) a.ScaleTs(alpha).Add(b.ScaleTs(1 - alpha));
}
public static bool Equals(this IMathVector a, IMathVector b, double tol)
{
var v = a.SubtractTs(b);
return(v.GetLength() < tol);
}
public static MathTransform GetRotationFromAxisAndAngle(this IMathUtility m, IMathVector axis, double angle)
{
return (MathTransform) m.CreateTransformRotateAxis(m.Origin(), axis, angle);
}
public static MathTransform GetTranslationFromVector(this IMathUtility m, IMathVector v)
{
var vData = v.ArrayData.CastArray<double>();
var xForm = new[] {1, 0, 0, 0, 1, 0, 0, 0, 1, vData[0], vData[1], vData[2], 1, 0, 0, 0};
return (MathTransform) m.CreateTransform(xForm);
}
public static MathVector Project(this ITriadManipulator m, swTriadManipulatorControlPoints_e h, IMathPoint p, IMathVector cameraVector, IMathUtility mathP)
{
IMathUtility math = mathP;
var zero = (IMathPoint)math.CreatePoint(new[] { 0, 0, 0 });
double pT, qT;
switch (h)
{
case swTriadManipulatorControlPoints_e.swTriadManipulatorOrigin:
return((MathVector)p.Subtract(m.Origin));
case swTriadManipulatorControlPoints_e.swTriadManipulatorXAxis:
return(ClosestPointOnAxis(m, p, cameraVector, m.XAxis));
case swTriadManipulatorControlPoints_e.swTriadManipulatorYAxis:
return(ClosestPointOnAxis(m, p, cameraVector, m.YAxis));
case swTriadManipulatorControlPoints_e.swTriadManipulatorZAxis:
return(ClosestPointOnAxis(m, p, cameraVector, m.ZAxis));
case swTriadManipulatorControlPoints_e.swTriadManipulatorXYPlane:
return((MathVector)p.Subtract(m.Origin));
case swTriadManipulatorControlPoints_e.swTriadManipulatorYZPlane:
return((MathVector)p.Subtract(m.Origin));
case swTriadManipulatorControlPoints_e.swTriadManipulatorZXPlane:
return((MathVector)p.Subtract(m.Origin));
default:
throw new ArgumentOutOfRangeException(nameof(h), h, null);
}
}
private static MathVector ClosestPointOnAxis(ITriadManipulator m, IMathPoint p, IMathVector cameraVector, MathVector axis)
{
double pT;
double qT;
if (ClosestPointBetweenLines(m.Origin, axis, p, cameraVector, out pT, out qT))
{
return((MathVector)axis.Scale(pT));
}
else
{
return(null);
}
}
private static MathVector ProjectRelative(IMathPoint origin, IMathVector axis, IMathPoint point)
{
return((MathVector)point.Project(origin, axis).Subtract(origin));
}
/// <summary>
/// http://geomalgorithms.com/a07-_distance.html
/// </summary>
/// <param name="pa"></param>
/// <param name="va"></param>
/// <param name="pb"></param>
/// <param name="vb"></param>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
private static bool ClosestPointBetweenLines(IMathPoint pOrigin, IMathVector pVector, IMathPoint qOrigin, IMathVector qVector, out double pT, out double qT)
{
var w0 = (IMathVector)(pOrigin.Subtract(qOrigin));
var u = pVector.Normalise();
var v = qVector.Normalise();
var a = u.Dot(u);
var b = u.Dot(v);
var c = v.Dot(v);
var d = u.Dot(w0);
var e = v.Dot(w0);
var den = (a * c - b * b);
if (Math.Abs(den) < 1e-12)
{
pT = 0;
qT = 0;
return(false);
}
pT = (b * e - c * d) / den;
qT = (a * a - b * d) / den;
return(true);
}
public static MathVector MultiplyTransformTs(this IMathVector v, IMathTransform t)
{
return((MathVector)v.MultiplyTransform(t));
}
public static MathVector CrossTs(this IMathVector a, IMathVector b)
{
return((MathVector)a.Cross(b));
}
public static MathVector AlphaBend(this IMathVector a, IMathVector b, double alpha)
{
Debug.Assert(alpha >= 0);
Debug.Assert(alpha <= 1);
return((MathVector)a.ScaleTs(alpha).Add(b.ScaleTs(1 - alpha)));
}
/// <summary>
/// Valida a integridade dos dados nos argumentos.
/// </summary>
/// <param name="initialDecompositionPoints">As estimativas iniciais relativas ao problema da decomposição.</param>
/// <param name="objectiveFunction">A função objectivo.</param>
/// <param name="cost">O custo inicial.</param>
/// <param name="masterConstraints">As restrições do problema principal.</param>
/// <param name="decomposedConstraints">As restrições dos vários problemas decompostos.</param>
/// <param name="inverseBasisMatrix">A matriz de base inversa.</param>
private void ValidateArguments(
IMathVector <ObjectiveCoeffType>[] initialDecompositionPoints,
IMathVector <ObjectiveCoeffType> objectiveFunction,
ObjectiveCoeffType cost,
LinearConstraintsInput <ConstraintsType> masterConstraints,
LinearConstraintsInput <ConstraintsType>[] decomposedConstraints,
ISquareMathMatrix <ConstraintsType> inverseBasisMatrix)
{
if (initialDecompositionPoints == null)
{
throw new ArgumentNullException("initialDecompositionPoints");
}
else if (objectiveFunction == null)
{
throw new ArgumentNullException("objectiveFunction");
}
else if (cost == null)
{
throw new ArgumentNullException("cost");
}
else if (masterConstraints == null)
{
throw new ArgumentNullException("masterConstraints");
}
else if (decomposedConstraints == null)
{
throw new ArgumentNullException("decomposedConstraints");
}
else if (inverseBasisMatrix == null)
{
throw new ArgumentNullException("inverseBasisMatrix");
}
else if (initialDecompositionPoints.Length != decomposedConstraints.Length)
{
throw new ArgumentException(
"The number of initial problem points must match the number of decomposition problem constraints.");
}
else
{
var length = decomposedConstraints.Length;
for (int i = 0; i < length; ++i)
{
var currentDecomposedConstraint = decomposedConstraints[i];
var currentPoint = initialDecompositionPoints[i];
if (currentDecomposedConstraint == null)
{
throw new ArgumentException("Null decomposition constraints aren't allowed.");
}
else if (currentPoint == null)
{
throw new ArgumentException("Null initial points aren't allowed.");
}
else
{
var constraintColumnsLength = currentDecomposedConstraint.ConstraintsMatrix.GetLength(1);
if (initialDecompositionPoints.Length != constraintColumnsLength)
{
throw new ArgumentException(
"The number of initial points coordinates must match the number of columns of the decomposition problem constraints matrix.");
}
}
}
}
}
/// <summary>
/// a X b => 投影到 b
/// gives the multiplier for b which would be the projection of a on b
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static double Project(this IMathVector a, IMathVector b)
{
return(a.Dot(b) / (b.Dot(b)));
}
public static IMathPoint RotateByAngle(this IMathUtility m, IMathPoint p, IMathVector axis, double angle)
{
var transformation = GetRotationFromAxisAndAngle(m, axis, angle);
return p.MultiplyTransformTs(transformation);
}
/// <summary>
/// Permite encontrar uma solução do problema da soma dos subconjunto.
/// </summary>
/// <remarks>
/// Dado um conjunto de valores A={a[1], a[2], a[3], ...}, encontrar um subconjunto B contido em A,
/// B={a[b1], a[b2], ...} de tal forma que a sua soma a[b1]+a[b2]+... seja menor ou igual que o valor.
/// O algoritmo implementado recorre à redução LLL.
/// </remarks>
/// <param name="coefficientValues">O conjunto de valores.</param>
/// <param name="sum">A soma a ser encontrada.</param>
/// <param name="reductionCoeff">O coeficiente de redução.</param>
/// <returns>O subconjunto do conjunto inicial cuja soma mais se aproxima do valor fornecido.</returns>
/// <exception cref="ArgumentNullException">Se algums dos argumentos for nulo.</exception>
public CoeffType[] Run(
CoeffType[] coefficientValues,
CoeffType sum,
NearestCoeffFieldType reductionCoeff)
{
if (coefficientValues == null)
{
throw new ArgumentNullException("coefficientValues");
}
else if (sum == null)
{
throw new ArgumentNullException("sum");
}
else
{
// Elabora o algoritmo
if (coefficientValues.Length > 0)
{
var vectorSpace = new VectorSpace <NearestCoeffFieldType>(
coefficientValues.Length + 1,
this.vectorFactory,
this.nearestField);
var lllReductionAlg = new LLLBasisReductionAlgorithm <IMathVector <NearestCoeffFieldType>, NearestCoeffFieldType, CoeffType>(
vectorSpace,
this.scalarProd,
this.nearest,
this.fieldComparer
);
// Constrói o conjunto de vectores a serem reduzidos.
var vectorSet = new IMathVector <NearestCoeffFieldType> [coefficientValues.Length + 1];
for (int i = 0; i < coefficientValues.Length; ++i)
{
var createdVector = this.vectorFactory.CreateVector(
coefficientValues.Length + 1,
this.nearestField.AdditiveUnity);
createdVector[i] = this.nearestField.MultiplicativeUnity;
createdVector[coefficientValues.Length] = this.nearestField.AdditiveInverse(
this.converter.InverseConversion(coefficientValues[i]));
vectorSet[i] = createdVector;
}
var lastVectorValue = this.vectorFactory.CreateVector(
coefficientValues.Length + 1,
this.nearestField.AdditiveUnity);
lastVectorValue[lastVectorValue.Length - 1] = this.converter.InverseConversion(sum);
vectorSet[coefficientValues.Length] = lastVectorValue;
var reduced = lllReductionAlg.Run(
vectorSet,
reductionCoeff);
var result = this.GetSolution(coefficientValues, reduced);
return(result);
}
else
{
// O tamanho do vector de coeficientes é zero.
return(new CoeffType[0]);
}
}
}
public static IMathPoint TranslateByVector(this IMathUtility m, IMathPoint p, IMathVector v)
{
var transformation = GetTranslationFromVector(m, v);
return p.MultiplyTransformTs(transformation);
}
/// <summary>
/// http://geomalgorithms.com/a07-_distance.html
/// </summary>
/// <param name="pa"></param>
/// <param name="va"></param>
/// <param name="pb"></param>
/// <param name="vb"></param>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
private static bool ClosestPointBetweenLines(IMathPoint pOrigin, IMathVector pVector, IMathPoint qOrigin, IMathVector qVector, out double pT, out double qT)
{
var w0 = (IMathVector) (pOrigin.Subtract(qOrigin));
var u = pVector.Normalise();
var v = qVector.Normalise();
var a = u.Dot(u);
var b = u.Dot(v);
var c = v.Dot(v);
var d = u.Dot(w0);
var e = v.Dot(w0);
var den = (a*c - b*b);
if (Math.Abs(den) < 1e-12)
{
pT = 0;
qT = 0;
return false;
}
pT = (b*e - c*d)/den;
qT = (a*a - b*d)/den;
return true;
}
/// <summary>
/// gives the multiplier for b which would be the projection of a on b
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static double Project(this IMathVector a, IMathVector b)
{
return a.Dot(b)/(b.Dot(b));
}
public static bool Equals(this IMathVector a, IMathVector b, double tol)
{
var v = a.SubtractTs(b);
return v.GetLength() < tol;
}
public static MathVector CrossTs(this IMathVector a, IMathVector b)
{
return (MathVector) a.Cross(b);
}
/// <summary>
/// Obtém o código confuso de um vector.
/// </summary>
/// <param name="obj">O vector.</param>
/// <returns>
/// O código confuso do vector adequado à utilização em alguns algoritmos habituais.
/// </returns>
public int GetHashCode(IMathVector <CoeffType> obj)
{
return(this.orderedColComparer.GetHashCode(obj));
}
private bool IsHorizontal(IMathVector normal)
{
double[] normalVecor = normal.ArrayData;
return Math.Abs(normalVecor[1]) > Math.Abs(normalVecor[0]) &&
Math.Abs(normalVecor[1]) > Math.Abs(normalVecor[2]) &&
Math.Abs(normalVecor[1]) > 0.5;
}
private static MathVector ProjectRelative(IMathPoint origin, IMathVector axis, IMathPoint point)
{
return (MathVector) point.Project(origin, axis).Subtract(origin);
}
public static MathVector SubtractTs(this IMathVector a, IMathVector b)
{
return (MathVector)a.Subtract(b);
}
public static MathPoint AddTs(this IMathPoint a, IMathVector b)
{
return((MathPoint)a.AddVector(b));
}
public static MathPoint AddTs(this IMathPoint a, IMathVector b)
{
return (MathPoint) a.AddVector(b);
}
/// <summary>
/// Determina se ambos os vectores são iguais.
/// </summary>
/// <param name="x">O primeiro vector a ser comparado.</param>
/// <param name="y">O segundo vector a ser comparado.</param>
/// <returns>
/// Verdadeiro se ambos os vectores forem iguais e falso caso contrário.
/// </returns>
public bool Equals(IMathVector <CoeffType> x, IMathVector <CoeffType> y)
{
return(this.orderedColComparer.Equals(x, y));
}
