/// <summary>
/// Generates a term that gets smaller as the given 2D point fits a 3D points set projection.
/// </summary>
/// <param name="pointsSet">A representation for the 3D points set</param>
/// <param name="point">The 2D point</param>
/// <returns>The term that measures fitness of <paramref name="point"/> being on the 2D projection of the set specified by <paramref name="pointsSet"/></returns>
public static Term Compute(CircleFeatureCurve pointsSet, Point point)
{
// here we explicitly assume that the view vector is (0, 0, 1) or (0, 0, -1)
var x_ = point.X;
var y_ = point.Y;
var cx = pointsSet.Center.X;
var cy = pointsSet.Center.Y;
var cz = pointsSet.Center.Z;
var nx = pointsSet.Normal.X;
var ny = pointsSet.Normal.Y;
var nz = pointsSet.Normal.Z;
var r = pointsSet.Radius;
var dx = cx - x_;
var dy = cy + y_;
var lhs = TermBuilder.Sum(
TermBuilder.Power(dx * nz, 2),
TermBuilder.Power(dy * nz, 2),
TermBuilder.Power(dx * nx + dy * ny, 2));
var rhs = TermBuilder.Power(r * nz, 2);
return(TermBuilder.Power(lhs - rhs, 2));
}
private static OptimizationProblem GetOptimizationProblem(IEnumerable <PrimitiveEditConstraint> constraints)
{
// generate constraint terms
var constraintTerms = new List <Term>();
var paramsToVars = new Dictionary <IEditParameter, ParameterVariable[]>();
foreach (var constraint in constraints)
{
TryAxisOnLine(constraintTerms, paramsToVars, constraint);
TryPointOnPlane(constraintTerms, paramsToVars, constraint);
}
// get variables
var variables =
(from vars in paramsToVars.Values
from var in vars
select var).ToArray();
// construct objective --> sum of squared distances to current values
var values = GetCurrentValues(variables);
var objective = TermUtils.SafeSum(
from i in Enumerable.Range(0, variables.Length)
select TermBuilder.Power(variables[i] - values[i], 2));
return(new OptimizationProblem
{
Constraints = constraintTerms.ToArray(),
Variables = variables,
Objective = objective,
});
}
public IEnumerable <double[]> Solve(Term objective, IEnumerable <Term> constraints, Variable[] variables, double[] startPoint)
{
var constraintSquares = constraints.Select(c => TermBuilder.Power(c, 2));
var penalizedObjective = objective + penaltyWeight * TermUtils.SafeSum(constraintSquares);
var compiledPenalizedObjective = penalizedObjective.Compile(variables);
var solution = unconstrainedOptimizer.Solve(x => compiledPenalizedObjective.Differentiate(x), startPoint, gradientNormThreshold);
yield return(solution);
}
public static Term DiffSquared(Term[] left, Term[] right)
{
Contract.Requires(left != null);
Contract.Requires(right != null);
Contract.Requires(left.Length == right.Length);
var toSum = from i in System.Linq.Enumerable.Range(0, left.Length)
select TermBuilder.Power(left[i] - right[i], 2);
return(TermBuilder.Sum(toSum));
}
protected override Tuple <Term, Term[]> Reconstruct(SnappedSphere snappedPrimitive, Dictionary <FeatureCurve, ISet <Annotation> > curvesToAnnotations)
{
var circle = CircleFitter.Fit(snappedPrimitive.ProjectionParallelCircle.SnappedTo.Points);
var result =
TermBuilder.Power(snappedPrimitive.Center.X - circle.Center.X, 2) +
TermBuilder.Power(snappedPrimitive.Center.Y + circle.Center.Y, 2) +
TermBuilder.Power(snappedPrimitive.Radius - circle.Radius, 2);
return(Tuple.Create(result, NO_TERMS));
}
public void PowerContract()
{
Assert.Throws <ArgumentNullException>(() => TermBuilder.Power(null, 1));
Assert.Throws <ArgumentNullException>(() => TermBuilder.Power(x, null));
Assert.Throws <ArgumentNullException>(() => TermBuilder.Power(null, x));
Assert.IsType <ConstPower>(TermBuilder.Power(x, 1));
Assert.Throws <ArgumentException>(() => TermBuilder.Power(x, double.NaN));
Assert.Throws <ArgumentException>(() => TermBuilder.Power(x, double.PositiveInfinity));
Assert.IsType <TermPower>(TermBuilder.Power(x, y));
Assert.IsType <TermPower>(TermBuilder.Power(1, y));
}
public static Term SoftMin(Term[] terms, double exponent = 6)
{
Contract.Requires(terms != null);
Contract.Requires(terms.Length > 0);
Contract.Requires(Contract.ForAll(terms, term => term != null));
Contract.Requires(exponent > 1);
Contract.Ensures(Contract.Result <Term>() != null);
var powers = terms.Select(term => TermBuilder.Power(term, -exponent));
return(TermBuilder.Power(TermBuilder.Sum(powers), -1 / exponent));
}
private static Term MeanSquaredError(Term[] terms, double[] values)
{
Contract.Requires(terms != null);
Contract.Requires(values != null);
Contract.Requires(terms.Length == values.Length);
Contract.Requires(Contract.ForAll(terms, term => term != null));
Contract.Ensures(Contract.Result <Term>() != null);
var errorTerms = from i in Enumerable.Range(0, terms.Length)
select TermBuilder.Power(terms[i] + (-values[i]), 2);
return((1 / (double)terms.Length) * TermUtils.SafeSum(errorTerms));
}
private Term[] GetConcreteAnnotationTerm(OnSphere onSphere)
{
var center = onSphere.SphereOwned.Center;
var radius = onSphere.SphereOwned.Radius;
var touchingPoint = onSphere.CenterTouchesSphere.Center;
var touchingNormal = onSphere.CenterTouchesSphere.Normal;
var radiusConstraint = (center - touchingPoint).NormSquared - TermBuilder.Power(radius, 2);
var normalConstraint = VectorParallelism(center - touchingPoint, touchingNormal);
return(normalConstraint.Append(radiusConstraint).ToArray());
}
private static Tuple <Term, Term[]> CreateSGCTerms(SnappedStraightGenCylinder snappedPrimitive, double[] radii, Point spineStart, Point spineEnd)
{
var constraints = new Term[] { snappedPrimitive.Axis.NormSquared - 1 };
if (radii.Length > 0)
{
var featureCurveTerms =
from item in snappedPrimitive.FeatureCurves.Cast <CircleFeatureCurve>()
where item != null
where item.SnappedTo != null
from term in ProjectionFit.Compute(item)
select term;
var featureCurvesTerm = 0.1 * TermUtils.SafeAvg(featureCurveTerms);
// the difference between the primitive's radii and the computed radii is minimized
var radiiApproxTerm = TermUtils.SafeAvg(
from i in Enumerable.Range(0, snappedPrimitive.Components.Length)
let component = snappedPrimitive.Components[i]
let radius = radii[i]
select TermBuilder.Power(component.Radius - radius, 2));
// the smoothness of the primitive's radii (laplacian norm) is minimized
var radiiSmoothTerm = TermUtils.SafeAvg(
from pair in snappedPrimitive.Components.SeqTripples()
let r1 = pair.Item1.Radius
let r2 = pair.Item2.Radius
let r3 = pair.Item3.Radius
select TermBuilder.Power(r2 - 0.5 * (r1 + r3), 2)); // how far is r2 from the avg of r1 and r3
// we specifically wish to give higher weight to first and last radii, so we have
// an additional first/last radii term.
var endpointsRadiiTerm =
TermBuilder.Power(radii[0] - snappedPrimitive.Components.First().Radius, 2) +
TermBuilder.Power(radii.Last() - snappedPrimitive.Components.Last().Radius, 2);
// objective - weighed average of all terms
var objective =
radiiApproxTerm +
radiiSmoothTerm +
featureCurvesTerm +
endpointsRadiiTerm;
return(Tuple.Create(objective, constraints));
}
else
{
return(Tuple.Create((Term)0, constraints)); // if we can't extract radii approximation, we don't do any snapping
}
}
private static ICompiledTerm ConstructTerm(Coefficient[][] coefficients, Variable[] variables)
{
var squareTerms =
from i in Enumerable.Range(0, coefficients.Length)
let termCoefficients = coefficients[i]
let sumTerms = from j in Enumerable.Range(0, termCoefficients.Length)
select termCoefficients[j].Value * variables[termCoefficients[j].Index]
let sum = TermBuilder.Sum(sumTerms)
select TermBuilder.Power(sum, 2);
var finalTerm = TermBuilder.Sum(squareTerms);
var compiled = finalTerm.Compile(variables);
return(compiled);
}
static void Main(string[] args)
{
var x = new Variable();
var y = new Variable();
var z = new Variable();
// f(x, y, z) = (x-2)² + (y+4)² + (z-1)²
// the min should be (x, y, z) = (2, -4, 1)
var func = TermBuilder.Power(x - 2, 2) + TermBuilder.Power(y + 4, 2) + TermBuilder.Power(z - 1, 2);
var compiled = func.Compile(x, y, z);
// perform optimization
var vec = new double[3];
vec = GradientDescent(compiled, vec, stepSize: 0.01, iterations: 1000);
Console.WriteLine("The approx. minimizer is: {0}, {1}, {2}", vec[0], vec[1], vec[2]);
}
protected Term EndpointsProjectionFit(CircleFeatureCurve pointsSet, Point p1, Point p2)
{
var x1 = p1.X;
var y1 = -p1.Y;
var x2 = p2.X;
var y2 = -p2.Y;
var cx = pointsSet.Center.X;
var cy = pointsSet.Center.Y;
var r = pointsSet.Radius;
var eq1 = TermBuilder.Power(x1 - cx, 2) + TermBuilder.Power(y1 - cy, 2) - TermBuilder.Power(r, 2);
var eq2 = TermBuilder.Power(x2 - cx, 2) + TermBuilder.Power(y2 - cy, 2) - TermBuilder.Power(r, 2);
var result = TermBuilder.Power(eq1, 2) + TermBuilder.Power(eq2, 2);
return(result);
}
private Term PrepareStDevFunction(int count = 5)
{
Term stDev = null;
for (int i = 0; i < count; ++i)
{
Variable w = new Variable();
if (!variables.ContainsKey(i))
{
variables.Add(i, w);
}
var std_mul_w = TermBuilder.Power(variables[i], 2) * TermBuilder.Power(stDevs[i], 2);
if (i == 0)
{
stDev = std_mul_w;
}
else
{
stDev += std_mul_w;
}
for (int k = i + 1; k < count; ++k)
{
Variable w2 = new Variable();
if (!variables.ContainsKey(k))
{
variables.Add(k, w2);
}
var cov = 2 * matCov[i, k] * variables[i] * variables[k];
stDev += cov;
}
}
stDev = TermBuilder.Power(stDev, 0.5);
return(stDev);
}
private static Tuple <Term, Term[]> CreateBGCTerms(SnappedBendedGenCylinder snappedPrimitive, double[] radii, Point[] medial_axis)
{
var terms =
from item in snappedPrimitive.FeatureCurves.Cast <CircleFeatureCurve>()
where item.SnappedTo != null
from term in ProjectionFit.Compute(item)
select term;
var featureCurvesTerm = TermUtils.SafeAvg(terms);
var radiiApproxTerm = TermUtils.SafeAvg(
from i in Enumerable.Range(0, snappedPrimitive.Components.Length)
let component = snappedPrimitive.Components[i]
let radius = radii[i]
select TermBuilder.Power(component.Radius - radius, 2));
// the smoothness of the primitive's radii (laplacian norm) is minimized
var radiiSmoothTerm = TermUtils.SafeAvg(
from pair in snappedPrimitive.Components.SeqTripples()
let r1 = pair.Item1.Radius
let r2 = pair.Item2.Radius
let r3 = pair.Item3.Radius
select TermBuilder.Power(r2 - 0.5 * (r1 + r3), 2)); // how far is r2 from the avg of r1 and r3
var positionSmoothnessTerm = TermUtils.SafeAvg(
from triple in snappedPrimitive.Components.SeqTripples()
let p1 = new TVec(triple.Item1.vS, triple.Item1.vT)
let p2 = new TVec(triple.Item2.vS, triple.Item2.vT)
let p3 = new TVec(triple.Item3.vS, triple.Item3.vT)
let laplacian = p2 - 0.5 * (p1 + p3)
select laplacian.NormSquared);
Term[] TermArray = new Term[medial_axis.Length];
for (int i = 0; i < medial_axis.Length; i++)
{
TVec PointOnSpine = snappedPrimitive.BottomCenter + snappedPrimitive.Components[i].vS * snappedPrimitive.U + snappedPrimitive.Components[i].vT * snappedPrimitive.V;
TermArray[i] = TermBuilder.Power(PointOnSpine.X - medial_axis[i].X, 2) +
TermBuilder.Power(PointOnSpine.Y + medial_axis[i].Y, 2);
}
var spinePointTerm = TermUtils.SafeAvg(TermArray);
// we specifically wish to give higher weight to first and last radii, so we have
// an additional first/last radii term.
var endpointsRadiiTerm =
TermBuilder.Power(radii[0] - snappedPrimitive.Components.First().Radius, 2) +
TermBuilder.Power(radii.Last() - snappedPrimitive.Components.Last().Radius, 2);
// objective - weighed average of all terms
var objective =
0.1 * featureCurvesTerm +
radiiApproxTerm +
radiiSmoothTerm +
positionSmoothnessTerm +
spinePointTerm +
endpointsRadiiTerm;
var sB = snappedPrimitive.NPbot.X;
var tB = snappedPrimitive.NPbot.Y;
var s0 = snappedPrimitive.Components[0].vS;
var t0 = snappedPrimitive.Components[0].vT;
var s1 = snappedPrimitive.Components[1].vS;
var t1 = snappedPrimitive.Components[1].vT;
var botNormalParallelism = tB * (s1 - s0) - sB * (t1 - t0);
var sT = snappedPrimitive.NPtop.X;
var tT = snappedPrimitive.NPtop.Y;
var sLast = snappedPrimitive.Components[snappedPrimitive.Components.Length - 1].vS;
var tLast = snappedPrimitive.Components[snappedPrimitive.Components.Length - 1].vT;
var sBeforeLast = snappedPrimitive.Components[snappedPrimitive.Components.Length - 2].vS;
var tBeforeLast = snappedPrimitive.Components[snappedPrimitive.Components.Length - 2].vT;
var topNormalParallelism = tT * (sLast - sBeforeLast) - sT * (tLast - tBeforeLast);
var topNormalized = snappedPrimitive.NPtop.NormSquared - 1;
var botNormalized = snappedPrimitive.NPbot.NormSquared - 1;
var uNormalized = snappedPrimitive.U.NormSquared - 1;
var vNormalized = snappedPrimitive.V.NormSquared - 1;
var uvOrthogonal = TVec.InnerProduct(snappedPrimitive.U, snappedPrimitive.V);
var firstComponentBottomS = snappedPrimitive.Components[0].vS;
var firstComponentBottomT = snappedPrimitive.Components[0].vT;
var constraints = new List <Term>
{
topNormalized,
botNormalized,
uNormalized,
vNormalized,
uvOrthogonal,
firstComponentBottomS,
firstComponentBottomT,
botNormalParallelism,
topNormalParallelism
};
var meanRadius = radii.Sum() / radii.Length;
var stdDevRadius = Math.Sqrt(radii.Select(r => r * r).Sum() / radii.Length - meanRadius * meanRadius);
Debug.WriteLine("ALEXALEX Mean radius is {0}, stddev is {1}", meanRadius, stdDevRadius);
if (stdDevRadius <= 0.3 * meanRadius)
{
foreach (var pair in snappedPrimitive.Components.SeqPairs())
{
var ra = pair.Item1.Radius;
var rb = pair.Item2.Radius;
constraints.Add(ra - rb);
}
}
return(Tuple.Create(objective, constraints.ToArray()));
}
protected override Tuple <Term, Term[]> Reconstruct(SnappedCuboid snappedPrimitive, Dictionary <FeatureCurve, ISet <Annotation> > curvesToAnnotations)
{
Point O = NewPrimitiveExtensions.FindCoincidentPoint(snappedPrimitive.CubicCorner);
Point A = NewPrimitiveExtensions.FindEndPoint(snappedPrimitive.CubicCorner[0].AssignedTo, O);
Point B = NewPrimitiveExtensions.FindEndPoint(snappedPrimitive.CubicCorner[1].AssignedTo, O);
Point C = NewPrimitiveExtensions.FindEndPoint(snappedPrimitive.CubicCorner[2].AssignedTo, O);
Point3D Op = NewPrimitiveExtensions.FindCoincident3DPoint(snappedPrimitive.CubicCorner);
EnhancedPrimitiveCurve ec = (EnhancedPrimitiveCurve)snappedPrimitive.CubicCorner[0];
Point3D Ap = NewPrimitiveExtensions.FindEnd3DPoint(ec.Points3D, Op);
ec = (EnhancedPrimitiveCurve)snappedPrimitive.CubicCorner[1];
Point3D Bp = NewPrimitiveExtensions.FindEnd3DPoint(ec.Points3D, Op);
ec = (EnhancedPrimitiveCurve)snappedPrimitive.CubicCorner[2];
Point3D Cp = NewPrimitiveExtensions.FindEnd3DPoint(ec.Points3D, Op);
O.Y = -O.Y;
A.Y = -A.Y;
B.Y = -B.Y;
C.Y = -C.Y;
Vector OA = A - O;
Vector OB = B - O;
Vector OC = C - O;
Debug.WriteLine("O=({0},{1})", O.X, O.Y);
Debug.WriteLine("A=({0},{1})", A.X, A.Y);
Debug.WriteLine("B=({0},{1})", B.X, B.Y);
Debug.WriteLine("C=({0},{1})", C.X, C.Y);
Vector OAn = OA.Normalized();
Vector OBn = OB.Normalized();
Vector OCn = OC.Normalized();
double dotABn = OAn * OBn;
double dotACn = OAn * OCn;
double dotBCn = OBn * OCn;
double a = Math.Acos(dotABn);
double b = Math.Acos(dotACn);
double c = Math.Acos(dotBCn);
/*double[] anglesSort = new double[] { a, b, c };
* double[] angles = new double[] { a, b, c };
* int[] idx = new int[3]{0, 1, 2};
* Vector[] vecArr = new Vector[] { OAn, OBn, OCn };
* Array.Sort(anglesSort, idx);
* Debug.WriteLine("(a,b,c)=({0},{1},{2})", a * 180 / Math.PI, b * 180 / Math.PI, c * 180 / Math.PI);
* Debug.WriteLine("sorted : (a,b,c)=({0},{1},{2})", anglesSort[0] * 180 / Math.PI, anglesSort[1] * 180 / Math.PI, anglesSort[2] * 180 / Math.PI);
* Debug.WriteLine("index : {0}, {1}, {2}", idx[0], idx[1], idx[2]);
* if (anglesSort[1] > Math.PI / 2 && anglesSort[0] < Math.PI / 2)
* {
* double theta = anglesSort[1] - Math.PI / 2 + 0.001;
* var rotMatrix = new RotateTransform(theta*180/Math.PI).Value;
* vecArr[idx[1]] = rotMatrix.Transform(vecArr[idx[1]]);
* angles[idx[1]] = Math.PI/2 - 0.001;
* angles[idx[0]] = angles[idx[2]] - angles[idx[1]];
* }
* Debug.WriteLine("after : (a,b,c)=({0},{1},{2},{3})", angles[0] * 180 / Math.PI, angles[1] * 180 / Math.PI, angles[2] * 180 / Math.PI, (angles[0]+angles[1])*180/Math.PI);
*
* dotABn = Math.Cos(angles[0]);
* dotACn = Math.Cos(angles[1]);
* dotBCn = Math.Cos(angles[2]);
*
* OAn = vecArr[0].Normalized();
* OBn = vecArr[1].Normalized();
* OCn = vecArr[2].Normalized();*/
Debug.WriteLine("Cubic Corner Index:{0}", snappedPrimitive.CubicCornerIdx);
double pn, qn, rn;
int signp = 0, signq = 0, signr = 0;
if (dotABn < 0 && dotACn < 0 && dotBCn < 0)
{
signp = signq = signr = 1;
}
else
{
if (dotABn < 0)
{
signp = signq = -1;
signr = 1;
}
else if (dotBCn < 0)
{
signq = signr = 1;
signp = -1;
}
else
{
signp = signr = 1;
signq = -1;
}
}
pn = signp * Math.Sqrt(-dotACn * dotABn / dotBCn);
qn = signq * Math.Sqrt(-dotABn * dotBCn / dotACn);
rn = signr * Math.Sqrt(-dotACn * dotBCn / dotABn);
Debug.WriteLine("p*q={0}, OA*OB={1}", pn * qn, dotABn);
Debug.WriteLine("p*r={0}, OA*OC={1}", pn * rn, dotACn);
Debug.WriteLine("q*r={0}, OB*OC={1}", rn * qn, dotBCn);
Vector3D OA3Dn = new Vector3D(OAn.X, OAn.Y, pn);
Vector3D OB3Dn = new Vector3D(OBn.X, OBn.Y, qn);
Vector3D OC3Dn = new Vector3D(OCn.X, OCn.Y, rn);
OA3Dn = OA3Dn.Normalized();
OB3Dn = OB3Dn.Normalized();
OC3Dn = OC3Dn.Normalized();
Vector3D pOA = (Ap - Op).Normalized();
Vector3D pOB = (Bp - Op).Normalized();
Vector3D pOC = (Cp - Op).Normalized();
Vector3D approxW = new Vector3D(); // = OA3Dn.Normalized();
Vector3D approxH = new Vector3D(); // = -OB3Dn.Normalized();
Vector3D approxD = new Vector3D(); // = OC3Dn.Normalized();
/*if (pOA.X * OA3Dn.X < 0) OA3Dn.X = -OA3Dn.X;
* if (pOA.Y * OA3Dn.Y < 0) OA3Dn.Y = -OA3Dn.Y;
* if (pOA.Z * OA3Dn.Z < 0) OA3Dn.Z = -OA3Dn.Z;
*
* if (pOB.X * OB3Dn.X < 0) OB3Dn.X = -OB3Dn.X;
* if (pOB.Y * OB3Dn.Y < 0) OB3Dn.Y = -OB3Dn.Y;
* if (pOB.Z * OB3Dn.Z < 0) OB3Dn.Z = -OB3Dn.Z;
*
* if (pOC.X * OC3Dn.X < 0) OC3Dn.X = -OC3Dn.X;
* if (pOC.Y * OC3Dn.Y < 0) OC3Dn.Y = -OC3Dn.Y;
* if (pOC.Z * OC3Dn.Z < 0) OC3Dn.Z = -OC3Dn.Z;*/
switch (snappedPrimitive.CubicCornerIdx)
{
case 0:
approxW = Vector3D.DotProduct(pOA, OA3Dn) > 0 ? OA3Dn : -OA3Dn;
approxH = Vector3D.DotProduct(pOB, OB3Dn) < 0 ? OB3Dn : -OB3Dn;
approxD = Vector3D.DotProduct(pOC, OC3Dn) > 0 ? OC3Dn : -OC3Dn;
break;
case 1:
approxW = Vector3D.DotProduct(pOA, OA3Dn) > 0 ? -OA3Dn : OA3Dn;
approxH = Vector3D.DotProduct(pOB, OB3Dn) > 0 ? -OB3Dn : OB3Dn;
approxD = Vector3D.DotProduct(pOC, OC3Dn) > 0 ? OC3Dn : -OC3Dn;
break;
case 2:
approxW = Vector3D.DotProduct(pOA, OA3Dn) > 0 ? -OA3Dn : OA3Dn;
approxH = Vector3D.DotProduct(pOB, OB3Dn) > 0 ? OB3Dn : -OB3Dn;
approxD = Vector3D.DotProduct(pOC, OC3Dn) < 0 ? OC3Dn : -OC3Dn;
break;
case 3:
approxW = -OA3Dn.Normalized();
approxH = -OB3Dn.Normalized();
approxD = -OC3Dn.Normalized();
break;
case 4:
approxW = OA3Dn.Normalized();
approxH = OB3Dn.Normalized();
approxD = OC3Dn.Normalized();
break;
case 5:
//if (pOA.X * OA3Dn.X < 0) OA3Dn.X = -OA3Dn.X;
//if (pOA.X * OA3Dn.X < 0) OA3Dn.X = -OA3Dn.X;
approxW = Vector3D.DotProduct(pOA, OA3Dn) > 0 ? -OA3Dn : OA3Dn;
approxH = Vector3D.DotProduct(pOB, OB3Dn) > 0 ? OB3Dn : -OB3Dn;
approxD = Vector3D.DotProduct(pOC, OC3Dn) > 0 ? OC3Dn : -OC3Dn;
//Debug.WriteLine();
break;
case 6:
approxW = OA3Dn.Normalized();
approxH = OB3Dn.Normalized();
approxD = -OC3Dn.Normalized();
break;
case 7:
approxW = -OA3Dn.Normalized();
approxH = OB3Dn.Normalized();
approxD = -OC3Dn.Normalized();
break;
}
snappedPrimitive.W = OA3Dn.Normalized();
snappedPrimitive.H = OB3Dn.Normalized();
snappedPrimitive.D = OC3Dn.Normalized();
Debug.WriteLine("Normalized W*H={0}", Vector3D.DotProduct(approxW, approxH));
Debug.WriteLine("Normalized W*D={0}", Vector3D.DotProduct(approxW, approxD));
Debug.WriteLine("Normalized D*H={0}", Vector3D.DotProduct(approxD, approxH));
/*double lOA = OA.Length;
* double lOB = OB.Length;
* double lOC = OC.Length;
* Debug.WriteLine("(lOA, lOB, lOC)=({0},{1},{2})", lOA, lOB, lOC);*/
//a = Math.Acos(OAn * OBn);
//b = Math.Acos(OAn * OCn);
//c = Math.Acos(OBn * OCn);
//Debug.WriteLine("(pi/2, a,b,c,sum)=({0},{1},{2}, {3},{4})", Math.PI / 2, a * 180 / Math.PI, b * 180 / Math.PI, c * 180 / Math.PI, a * 180 / Math.PI + b * 180 / Math.PI);
/*double cota = 1 / Math.Tan(a);
* double cotb = 1 / Math.Tan(b);
* double cotc = 1 / Math.Tan(c);*/
/*double dotAB = lOA * lOB * Math.Cos(a);
* double dotAC = lOA * lOC * Math.Cos(b);
* double dotBC = lOB * lOC * Math.Cos(c);*/
double dotAB = OA * OB;
double dotAC = OA * OC;
double dotBC = OB * OC;
double p, q, r;
p = signp * Math.Sqrt(-dotAC * dotAB / dotBC);
q = signq * Math.Sqrt(-dotAB * dotBC / dotAC);
r = signr * Math.Sqrt(-dotAC * dotBC / dotAB);
//double r = Math.Sqrt(-dotAB*dotBC/dotAC);
//Debug.WriteLine("(p,q,r)=({0},{1},{2})", p, q, r);
//ec = (EnhancedPrimitiveCurve)snappedPrimitive.CubicCorner[0];
Vector3D cuboidOA = ec.Points3D[1] - ec.Points3D[0];
//ec = (EnhancedPrimitiveCurve)snappedPrimitive.CubicCorner[1];
Vector3D cuboidOB = ec.Points3D[1] - ec.Points3D[0];
//ec = (EnhancedPrimitiveCurve)snappedPrimitive.CubicCorner[2];
Vector3D cuboidOC = ec.Points3D[1] - ec.Points3D[0];
double zAp = p;
double zAn = -zAp;
double zBp = q;
double zBn = -zBp;
double zCp = r;
double zCn = -zCp;
Point3D O3D = new Point3D(O.X, O.Y, 0);
snappedPrimitive.Origin = O3D;
Point3D A3Dp = new Point3D(A.X, A.Y, zAp);
Point3D A3Dn = new Point3D(A.X, A.Y, -zAp);
Vector3D OA3Dp = A3Dp - O3D;
//Point3D A3D = (Vector3D.DotProduct(cuboidOA, OA3Dp) > 0) ? A3Dp : A3Dn;
Point3D A3D = A3Dp;// (Vector3D.DotProduct(cuboidOA, OA3Dp) > 0) ? A3Dp : A3Dn;
//Vector
Point3D B3Dp = new Point3D(B.X, B.Y, zBp);
Point3D B3Dn = new Point3D(B.X, B.Y, -zBp);
Vector3D OB3Dp = B3Dp - O3D;
//Point3D B3D = (Vector3D.DotProduct(cuboidOB, OB3Dp) > 0) ? B3Dp : B3Dn;
Point3D B3D = B3Dp; //(Vector3D.DotProduct(cuboidOB, OB3Dp) > 0) ? B3Dp : B3Dn;
Point3D C3Dp = new Point3D(C.X, C.Y, zCp);
Point3D C3Dn = new Point3D(C.X, C.Y, -zCp);
Vector3D OC3Dp = C3Dp - O3D;
//Point3D C3D = (Vector3D.DotProduct(cuboidOC, OC3Dp) > 0) ? C3Dp : C3Dn;
Point3D C3D = C3Dp;
Debug.WriteLine("OA3D=({0},{1},{2})", OA3Dp.X, OA3Dp.Y, OA3Dp.Z);
Debug.WriteLine("OB3D=({0},{1},{2})", OB3Dp.X, OB3Dp.Y, OB3Dp.Z);
Debug.WriteLine("W*H={0}", Vector3D.DotProduct(approxH, approxW));
Debug.WriteLine("W*D={0}", Vector3D.DotProduct(approxD, approxW));
Debug.WriteLine("D*H={0}", Vector3D.DotProduct(approxH, approxD));
double approxWidth = (A3D - O3D).Length;
double approxHeight = (O3D - B3D).Length;
double approxDepth = (C3D - O3D).Length;
Point3D approxCenter = new Point3D();
switch (snappedPrimitive.CubicCornerIdx)
{
case 0:
approxCenter = O3D + 0.5 * approxWidth * approxW - 0.5 * approxHeight * approxH + 0.5 * approxDepth * approxD;
break;
case 1:
approxCenter = O3D - 0.5 * approxWidth * approxW - 0.5 * approxHeight * approxH + 0.5 * approxDepth * approxD;
break;
case 2:
approxCenter = O3D + 0.5 * approxWidth * approxW - 0.5 * approxHeight * approxH - 0.5 * approxDepth * approxD;
break;
case 3:
approxCenter = O3D - 0.5 * approxWidth * approxW - 0.5 * approxHeight * approxH - 0.5 * approxDepth * approxD;
break;
case 4:
approxCenter = O3D + 0.5 * approxWidth * approxW + 0.5 * approxHeight * approxH + 0.5 * approxDepth * approxD;
break;
case 5:
approxCenter = O3D - 0.5 * approxWidth * approxW + 0.5 * approxHeight * approxH + 0.5 * approxDepth * approxD;
break;
case 6:
approxCenter = O3D + 0.5 * approxWidth * approxW + 0.5 * approxHeight * approxH - 0.5 * approxDepth * approxD;
break;
case 7:
approxCenter = O3D - 0.5 * approxWidth * approxW + 0.5 * approxHeight * approxH - 0.5 * approxDepth * approxD;
break;
}
var CenterTerm =
TermBuilder.Power(snappedPrimitive.Center.X - approxCenter.X, 2) +
TermBuilder.Power(snappedPrimitive.Center.Y - approxCenter.Y, 2) +
TermBuilder.Power(snappedPrimitive.Center.Z - approxCenter.Z, 2);
var LengthTerm =
TermBuilder.Power(snappedPrimitive.Width - approxWidth, 2) +
TermBuilder.Power(snappedPrimitive.Height - approxHeight, 2) +
TermBuilder.Power(snappedPrimitive.Depth - approxDepth, 2);
double widthVectorArea = approxHeight * approxDepth; // area of the face orthogonal to width vector
double heightVectorArea = approxWidth * approxDepth; // area of the face orthogonal to height vector
double depthVectorArea = approxWidth * approxHeight; // area of the face orthogonal to depth vector
Debug.WriteLine("WVA = {0}, HVA = {1}, DVA = {2}", widthVectorArea, heightVectorArea, depthVectorArea);
var WidthVectorTerm = Math.Pow(widthVectorArea, 2) * (
TermBuilder.Power(snappedPrimitive.Wv.X - approxW.X, 2) +
TermBuilder.Power(snappedPrimitive.Wv.Y - approxW.Y, 2) +
TermBuilder.Power(snappedPrimitive.Wv.Z - approxW.Z, 2));
var HeightVectorTerm = Math.Pow(heightVectorArea, 2) * (
TermBuilder.Power(snappedPrimitive.Hv.X - approxH.X, 2) +
TermBuilder.Power(snappedPrimitive.Hv.Y - approxH.Y, 2) +
TermBuilder.Power(snappedPrimitive.Hv.Z - approxH.Z, 2));
var DepthVectorTerm = Math.Pow(depthVectorArea, 2) * (
TermBuilder.Power(snappedPrimitive.Dv.X - approxD.X, 2) +
TermBuilder.Power(snappedPrimitive.Dv.Y - approxD.Y, 2) +
TermBuilder.Power(snappedPrimitive.Dv.Z - approxD.Z, 2));
var objective =
CenterTerm +
LengthTerm +
100 * (WidthVectorTerm +
HeightVectorTerm +
DepthVectorTerm);
var ABorthogonal = TVec.InnerProduct(snappedPrimitive.Wv, snappedPrimitive.Hv);
var ACorthogonal = TVec.InnerProduct(snappedPrimitive.Wv, snappedPrimitive.Dv);
var BCorthogonal = TVec.InnerProduct(snappedPrimitive.Hv, snappedPrimitive.Dv);
var constraints = new Term[] { snappedPrimitive.Wv.NormSquared - 1, snappedPrimitive.Hv.NormSquared - 1, snappedPrimitive.Dv.NormSquared - 1, ABorthogonal, ACorthogonal, BCorthogonal };
return(Tuple.Create(10 * objective, constraints));
}
protected override Term GetRadiusSoftConstraint(SnappedCone snapped, double expectedTop, double expectedBottom)
{
return(0.5 * (
TermBuilder.Power(snapped.TopRadius - expectedTop, 2) +
TermBuilder.Power(snapped.BottomRadius - expectedBottom, 2)));
}
private AutoDiff.Term ConvertToAutoDiff(ISymbolicExpressionTreeNode node)
{
if (node.Symbol is Constant)
{
initialConstants.Add(((ConstantTreeNode)node).Value);
var var = new AutoDiff.Variable();
variables.Add(var);
return(var);
}
if (node.Symbol is Variable || node.Symbol is BinaryFactorVariable)
{
var varNode = node as VariableTreeNodeBase;
var factorVarNode = node as BinaryFactorVariableTreeNode;
// factor variable values are only 0 or 1 and set in x accordingly
var varValue = factorVarNode != null ? factorVarNode.VariableValue : string.Empty;
var par = FindOrCreateParameter(parameters, varNode.VariableName, varValue);
if (makeVariableWeightsVariable)
{
initialConstants.Add(varNode.Weight);
var w = new AutoDiff.Variable();
variables.Add(w);
return(AutoDiff.TermBuilder.Product(w, par));
}
else
{
return(varNode.Weight * par);
}
}
if (node.Symbol is FactorVariable)
{
var factorVarNode = node as FactorVariableTreeNode;
var products = new List <Term>();
foreach (var variableValue in factorVarNode.Symbol.GetVariableValues(factorVarNode.VariableName))
{
var par = FindOrCreateParameter(parameters, factorVarNode.VariableName, variableValue);
initialConstants.Add(factorVarNode.GetValue(variableValue));
var wVar = new AutoDiff.Variable();
variables.Add(wVar);
products.Add(AutoDiff.TermBuilder.Product(wVar, par));
}
return(AutoDiff.TermBuilder.Sum(products));
}
if (node.Symbol is LaggedVariable)
{
var varNode = node as LaggedVariableTreeNode;
var par = FindOrCreateParameter(parameters, varNode.VariableName, string.Empty, varNode.Lag);
if (makeVariableWeightsVariable)
{
initialConstants.Add(varNode.Weight);
var w = new AutoDiff.Variable();
variables.Add(w);
return(AutoDiff.TermBuilder.Product(w, par));
}
else
{
return(varNode.Weight * par);
}
}
if (node.Symbol is Addition)
{
List <AutoDiff.Term> terms = new List <Term>();
foreach (var subTree in node.Subtrees)
{
terms.Add(ConvertToAutoDiff(subTree));
}
return(AutoDiff.TermBuilder.Sum(terms));
}
if (node.Symbol is Subtraction)
{
List <AutoDiff.Term> terms = new List <Term>();
for (int i = 0; i < node.SubtreeCount; i++)
{
AutoDiff.Term t = ConvertToAutoDiff(node.GetSubtree(i));
if (i > 0)
{
t = -t;
}
terms.Add(t);
}
if (terms.Count == 1)
{
return(-terms[0]);
}
else
{
return(AutoDiff.TermBuilder.Sum(terms));
}
}
if (node.Symbol is Multiplication)
{
List <AutoDiff.Term> terms = new List <Term>();
foreach (var subTree in node.Subtrees)
{
terms.Add(ConvertToAutoDiff(subTree));
}
if (terms.Count == 1)
{
return(terms[0]);
}
else
{
return(terms.Aggregate((a, b) => new AutoDiff.Product(a, b)));
}
}
if (node.Symbol is Division)
{
List <AutoDiff.Term> terms = new List <Term>();
foreach (var subTree in node.Subtrees)
{
terms.Add(ConvertToAutoDiff(subTree));
}
if (terms.Count == 1)
{
return(1.0 / terms[0]);
}
else
{
return(terms.Aggregate((a, b) => new AutoDiff.Product(a, 1.0 / b)));
}
}
if (node.Symbol is Absolute)
{
var x1 = ConvertToAutoDiff(node.GetSubtree(0));
return(abs(x1));
}
if (node.Symbol is AnalyticQuotient)
{
var x1 = ConvertToAutoDiff(node.GetSubtree(0));
var x2 = ConvertToAutoDiff(node.GetSubtree(1));
return(x1 / (TermBuilder.Power(1 + x2 * x2, 0.5)));
}
if (node.Symbol is Logarithm)
{
return(AutoDiff.TermBuilder.Log(
ConvertToAutoDiff(node.GetSubtree(0))));
}
if (node.Symbol is Exponential)
{
return(AutoDiff.TermBuilder.Exp(
ConvertToAutoDiff(node.GetSubtree(0))));
}
if (node.Symbol is Square)
{
return(AutoDiff.TermBuilder.Power(
ConvertToAutoDiff(node.GetSubtree(0)), 2.0));
}
if (node.Symbol is SquareRoot)
{
return(AutoDiff.TermBuilder.Power(
ConvertToAutoDiff(node.GetSubtree(0)), 0.5));
}
if (node.Symbol is Cube)
{
return(AutoDiff.TermBuilder.Power(
ConvertToAutoDiff(node.GetSubtree(0)), 3.0));
}
if (node.Symbol is CubeRoot)
{
return(cbrt(ConvertToAutoDiff(node.GetSubtree(0))));
}
if (node.Symbol is Sine)
{
return(sin(
ConvertToAutoDiff(node.GetSubtree(0))));
}
if (node.Symbol is Cosine)
{
return(cos(
ConvertToAutoDiff(node.GetSubtree(0))));
}
if (node.Symbol is Tangent)
{
return(tan(
ConvertToAutoDiff(node.GetSubtree(0))));
}
if (node.Symbol is HyperbolicTangent)
{
return(tanh(
ConvertToAutoDiff(node.GetSubtree(0))));
}
if (node.Symbol is Erf)
{
return(erf(
ConvertToAutoDiff(node.GetSubtree(0))));
}
if (node.Symbol is Norm)
{
return(norm(
ConvertToAutoDiff(node.GetSubtree(0))));
}
if (node.Symbol is StartSymbol)
{
if (addLinearScalingTerms)
{
// scaling variables α, β are given at the beginning of the parameter vector
var alpha = new AutoDiff.Variable();
var beta = new AutoDiff.Variable();
variables.Add(beta);
variables.Add(alpha);
var t = ConvertToAutoDiff(node.GetSubtree(0));
return(t * alpha + beta);
}
else
{
return(ConvertToAutoDiff(node.GetSubtree(0)));
}
}
throw new ConversionException();
}
protected override Term GetRadiusSoftConstraint(SnappedCylinder snapped, double expectedTop, double expectedBottom)
{
var avg = 0.5 * (expectedTop + expectedBottom);
return(TermBuilder.Power(snapped.Radius - avg, 2));
}
