public static GridDirection GetDirectionFromRotation(Angle pitch, Angle yaw, Angle roll)
        {
            Vector3 v = new Vector3(1, 0, 0);

            v.Pitch(pitch);
            v.Yaw(yaw);
            v.Roll(roll);

            v.X = Math.Round(v.X, 5);
            v.Y = Math.Round(v.Y, 5);
            v.Z = Math.Round(v.Z, 5);

            double val = Math.Sqrt(2 - Math.Sqrt(2)) / 2;

            if (v.X <= 1 && v.X >= val) v.X = 1;
            else
                if (v.X <= val && v.X >= 0) v.X = 0;
                else
                    if (v.X >= -1 && v.X <= -val) v.X = -1;
                    else
                        if (v.X >= -val && v.X <= 0) v.X = 0;

            if (v.Y <= 1 && v.Y >= val) v.Y = -1;
            else
                if (v.Y <= val && v.Y >= 0) v.Y = 0;
                else
                    if (v.Y >= -1 && v.Y <= -val) v.Y = 1;
                    else
                        if (v.Y >= -val && v.Y <= 0) v.Y = 0;

            if (v.Z <= 1 && v.Z >= val) v.Z = 1;
            else
                if (v.Z <= val && v.Z >= 0) v.Z = 0;
                else
                    if (v.Z >= -1 && v.Z <= -val) v.Z = -1;
                    else
                        if (v.Z >= -val && v.Z <= 0) v.Z = 0;

            return v.AsDirection();
        }
Пример #2
0
        public static IEnumerable<GCodeCommand> ApplyHeightMap(this IEnumerable<GCodeCommand> commands, HeightMap map)
        {
            foreach (GCodeCommand command in commands)
            {
                if (command is OtherCode)
                {
                    yield return command;
                    continue;
                }
                else
                {
                    Movement m = (Movement)command;

                    int divisions = (int)Math.Ceiling(m.Length / map.GridSize);

                    if (m is Straight)
                    {
                        Straight s = (Straight)m;

                        if (s.Rapid)
                        {
                            Vector3 newEnd = s.End;
                            newEnd.Z += map.GetHeightAt(s.End.X, s.End.Y);

                            yield return new Straight(s.Start, newEnd, true);
                        }
                        else
                        {
                            Vector3 pos = s.Start;

                            for (int x = 1; x <= divisions; x++)
                            {
                                Vector3 end = s.Start.Interpolate(s.End, (float)x / (float)divisions);
                                end.Z += map.GetHeightAt(end.X, end.Y);
                                Straight st = new Straight(pos, end, false);
                                if (x == 1)
                                    st.FeedRate = s.FeedRate;
                                yield return st;
                                pos = end;
                            }
                        }
                    }
                    if (m is Arc)
                    {
                        Arc a = (Arc)m;

                        Vector3 pos = a.Start;

                        float stretch = a.StartAngle - a.EndAngle;

                        if (stretch <= 0)
                            stretch += 2 * (float)Math.PI;

                        if (a.Direction == ArcDirection.CCW)
                        {
                            stretch = 2 * (float)Math.PI - stretch;
                        }

                        if (stretch <= 0)
                            stretch += 2 * (float)Math.PI;

                        for (int x = 1; x <= divisions; x++)
                        {
                            Vector3 end = new Vector3(a.Radius, 0, 0);

                            if (a.Direction != ArcDirection.CW)
                                end.Roll(a.StartAngle + stretch * (float)x / (float)divisions);
                            else
                                end.Roll(a.StartAngle - stretch * (float)x / (float)divisions);

                            end += a.Center;

                            end.Z = a.Start.Z + (a.End.Z - a.Start.Z) * (float)x / (float)divisions;

                            end.Z += map.GetHeightAt(end.X, end.Y);

                            Arc arc = new Arc(pos, end, a.Center, a.Direction);

                            if (x == 1)
                                arc.FeedRate = a.FeedRate;

                            yield return arc;
                            pos = end;
                        }
                    }
                }
            }

            yield break;
        }
        public static GridDirection GetDirectionFromRotation(int pitch, int yaw, int roll)
        {
            Vector3 v = new Vector3(1, 0, 0);

            v.Pitch(new Angle(1, pitch, 0, 0));
            v.Yaw(new Angle(1, yaw, 0, 0));
            v.Roll(new Angle(1, roll, 0, 0));

            v.X = Math.Round(v.X, 5);
            v.Y = Math.Round(v.Y, 5);
            v.Z = Math.Round(v.Z, 5);

            double val = Math.Sqrt(2 - Math.Sqrt(2))/2;

            if (v.X <= 1 && v.X >= val) v.X = 1;
            else
            if (v.X <= val && v.X >= 0) v.X = 0;
            else
            if (v.X >= -1 && v.X <= -val) v.X = -1;
            else
            if (v.X >= -val && v.X <= 0) v.X = 0;

            if (v.Y <= 1 && v.Y >= val) v.Y = -1;
            else
            if (v.Y <= val && v.Y >= 0) v.Y = 0;
            else
            if (v.Y >= -1 && v.Y <= -val) v.Y = 1;
            else
            if (v.Y >= -val && v.Y <= 0) v.Y = 0;

            if (v.Z <= 1 && v.Z >= val) v.Z = 1;
            else
            if (v.Z <= val && v.Z >= 0) v.Z = 0;
            else
            if (v.Z >= -1 && v.Z <= -val) v.Z = -1;
            else
            if (v.Z >= -val && v.Z <= 0) v.Z = 0;

            return v.AsDirection();
        }