C# Plane.Transform方法代码示例

            public override void solveIK(Plane ep, int sol)
            {
                //List<Point3d> points = new List<Point3d>();

                double a1, a2, a3, a4, a5, a6;
                a1 = a2 = a3 = a4 = a5 = a6 = 0.0;

                ep.Transform(Transform.PlaneToPlane(this.Position, Plane.WorldXY));
                ep.Transform(this.Tool.TCPToRoot);

                Point3d wc = ep.Origin * 0.001 - ep.ZAxis * 0.175;  // wrist centre
                //points.Add(wc);
                //lines.Add(new Line(wc * 1000, TF.Origin));

                /* vector from frame centre to wrist centre */
                Vector3d wc_o = ep.ZAxis * -0.175;

                /* project wc onto xy plane */
                Vector3d wc_xy = new Vector3d(wc.X, wc.Y, 0.0);
                double d_wc_xy = wc_xy.Length;
                wc_xy.Unitize();

                /* ### A1 ### */
                a1 = -Math.Atan2(wc.Y, wc.X);

                Vector3d v12 = new Vector3d(0.3520, 0.0, 0.831); // vector from root to shoulder
                // rotate v12 in line with wc_xy
                //points.Add(new Point3d(v12.X * wc_xy.X, v12.X * wc_xy.Y, v12.Z));

                Vector3d v35 = new Vector3d(0.826, 0.0, 0.1535); // vector from elbow to wrist centre (a3 to a5)

                double d23 = 0.825;       // distance from shoulder joint to elbow joint (a2 to a3)
                double d35 = v35.Length;  // distance from elbow to wrist centre (a3 to a5)
                double s35 = Math.Atan2(v35.Z, v35.X);

                /* to account for other solution */
                /* [0] [1] : v2wc = new Vector3d(d_wc_xy - v12.X + (v12.X * 2 * i), wc.Z - v12.Z);*/
                Vector3d v2wc = new Vector3d(d_wc_xy - v12.X, 0.0, wc.Z - v12.Z); // shoulder to wrist centre, 2d
                double d2wc = v2wc.Length;

                /* ### A3 ### */
                a3 = Math.PI - (Math.Acos((d23 * d23 + d35 * d35 - d2wc * d2wc) / (2 * d23 * d35)) - s35);

                double a3_c = Math.Acos((d23 * d23 + d2wc * d2wc - d35 * d35) / (2 * d23 * d2wc));
                double a2_temp = Math.Atan2(v2wc.Z, v2wc.X);

                /* ### A2 ### */
                a2 = -a3_c - a2_temp;

                /* vector straight down the arm from a3 to a5 / wc */
                /* construct plane perpendicular to A3 */
                double beta = -a3 - a2 - Math.PI;  // get angle between forearm and XY plane (check sketchbook)
                Vector3d v3wc = new Vector3d(wc_xy.X, wc_xy.Y, Math.Tan(beta)); // construct 3d vector using wc_xy

                Vector3d p3wc_x = Vector3d.CrossProduct(new Vector3d(0, 0, 1), v3wc);
                Vector3d p3wc_y = Vector3d.CrossProduct(v3wc, p3wc_x);

                /* project wc_o onto new plane created by p3wc_x and p3wc_y */
                v3wc.Unitize();
                Vector3d proj_4 = wc_o - Vector3d.Multiply(wc_o, v3wc) * v3wc;

                /* ### A4 ### */
                proj_4.Unitize();
                p3wc_y.Unitize();
                double c4 = Vector3d.Multiply(-proj_4, p3wc_y);

                a4 = Math.Acos(c4);

                Vector3d p5wc_n = Vector3d.CrossProduct(proj_4, v3wc); // normal of plane created by v3wc and the flattened vector we just created
                p5wc_n.Unitize();
                Vector3d proj_5 = wc_o - Vector3d.Multiply(wc_o, p5wc_n) * p5wc_n;
                proj_5.Unitize();

                /* ### A5 ### */
                double c5 = Vector3d.Multiply(proj_5, proj_4);
                a5 = -0.5 * Math.PI + Math.Acos(c5);

                List<double> angles = new List<double>();

                angles.Add(a1);
                angles.Add(a2);
                angles.Add(a3);
                angles.Add(a4);
                angles.Add(a5);
                angles.Add(a6);

                for (int j = 0; j < this.Joints.Count - 1; ++j)
                {
                    this.Joints[j + 1].Angle(angles[j]);
                }
            }

 public override double[] PlaneToNumbers(Plane plane)
 {
     // Plane originPlane = new Plane(Point3d.Origin, Vector3d.YAxis, -Vector3d.XAxis);
     Plane originPlane = Plane.WorldXY;
     plane.Transform(Transform.PlaneToPlane(Plane.WorldXY, originPlane));
     Point3d point = plane.Origin / 1000;
     plane.Origin = point;
     double[] axisAngle = PlaneToAxisAngle(plane, Plane.WorldXY);
     return axisAngle;
 }

        public List<Line> Offset3D(List<Polyline> x, double y)
        {
            List<Line> output = new List<Line>();
            if (x.Count < 4) return output;
            List<Line> lines = breakPoly(x[0]);

            for (int i = 1; i < x.Count; i++)
            {
                List<Line> ls = breakPoly(x[i]);
                //Print(ls.Count.ToString());
                for (int ii = 0; ii < ls.Count; ii++)
                {
                    bool sign = true;
                    for (int j = 0; j < lines.Count; j++)
                    {
                        if (isDumpLines(lines[j], ls[ii])) { sign = false; break; }
                    }
                    //Print(sign.ToString());
                    if (sign) lines.Add(ls[ii]);
                }
            }
            Point3d cen = new Point3d();
            for (int i = 0; i < lines.Count; i++)
            {
                cen += lines[i].From; cen += lines[i].To;
            }
            // B = lines;
            cen /= 2 * lines.Count;
            HullFrame.box box = new HullFrame.box(lines);
            HullFrame.hull hull = new HullFrame.hull(box, cen);
            for (int i = 0; i < x.Count; i++)
            {
                if (x[i].Count < 3)
                {//Print("00001");
                    return output;
                }
                Plane p = new Plane(x[i][0], x[i][1], x[i][2]);
                Vector3d v = cen - p.ClosestPoint(cen);
                v.Unitize(); p = new Plane(x[i][0], v);
                p.Transform(Transform.Translation(v * y));
                hull.intersect(p);
                hull.clearnull();
            }

            for (int i = 0; i < hull.edges.Count; i++)
            {
                output.Add(new Line(hull.edges[i].p1.pos, hull.edges[i].p2.pos));
            }
            List<Point3d> pt = new List<Point3d>();
            for (int i = 0; i < hull.pts.Count; i++)
            {
                pt.Add(hull.pts[i].pos);
            }
            return output;
        }