private static Term ConstructAugmentedLagrangian(Term objective, IEnumerable <Term> constraints, Variable[] multipliers, Variable mu) { var constraintsVec = new TVec(constraints); var multipliersVec = new TVec(multipliers); var augmentedLagrangian = objective + TVec.InnerProduct(multipliersVec, constraintsVec) + 0.5 * mu * constraintsVec.NormSquared; return(augmentedLagrangian); }
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)); }
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())); }