Trigonometry (or maybe accuracy error?) problems with C#

I'm trying to make a program that can make animations - by drawing on screen a Stick-figure, and allowing the user to move its arms and legs, et cetera.

But if I want to move a part (or the figure) that is connected with both ends to another part, I need to move that part too, and maintain the same angle between them, so it would look realistic.

I have this code (assuming that input is OK, a.k.a the radius of the moving part is the same.)

public void MoveParts(double newX, double newY)
{
    foreach (var part in InnerParts)
    {
        var angle = GetAngle(part);
        var radius = GetDistace(part.baseX,part.baseY,part.x,part.y);
        part.baseX = newX;
        part.baseY = newY;
        var t = Math.Atan2(this.y - this.baseY, this.x - this.baseX);开发者_如何学JAVA //curr angle
        angle = 2 * Math.PI - angle + t; //proved via geometry
        part.x = part.baseX + Math.Cos(angle) * radius;
        part.y = part.baseY + Math.Sin(angle) * radius;
    }
    this.x = newX;
    this.y = newY;
}

private double GetAngle(Body part)
{
    return Math.Atan2(part.y - part.baseY, part.x - part.baseX) - Math.Atan2(this.baseY - this.y, this.baseX - this.x);
}
private double GetDistace(double x1, double y1, double x2, double y2)
{
    return Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2));
}

The problem is, that the moved part (not the base part), at each update, has either its origin value (between the base part) - or a different, value, changing every frame - although once in two frames it seems to work ok.

As a programmer I can just skip it by doing two updates every frame, but as a geek I wanna know where I went wrong.

Anyone that have good geometry knowledge, can you help me?

Thanks, Mark. Image (frames are counted):

Though this isn't a solution to your problem, one thing I'd change is this:

    private double GetDistace(double x1, double y1, double x2, double y2)
    {
        return Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2));
    }

to this:

    private double GetDistace(double x1, double y1, double x2, double y2)
    {
        double xd = x1 - x2;
        doubly yd = y1 - y2;
        return Math.Sqrt(xd * xd + yd * yd);
    }

It should be both quicker and (possibly) more accurate than Math.Pow.

I am aware that this is strictly not an answer to OPs question but perhaps it can prove interesting anyway.

So I read your code and tried to figure out what the problem is but I started to think: "I wonder if what OP really need is a SceneGraph".

Now I am not an expert on SceneGraphs but I thought I could demonstrate perhaps a different approach building up composite visual objects.

By structuring the objects in this way it's simpler to build up and maintain complex structures when rotating and translating.

Start off by defining an abstract class Shape

abstract class Shape
{
    static readonly Shape[] s_emptyChildren = new Shape[0];

    public Rect BoundingBox;
    public Transform Transform;
    public Shape[] Children = s_emptyChildren;

    public void RenderShape (DrawingContext context)
    {
        context.PushTransform (Transform ?? Transform.Identity);

        try
        {
            OnRenderShape (context);

            foreach (var shape in Children ?? s_emptyChildren)
            {
                shape.RenderShape (context);
            }
        }
        finally
        {
            context.Pop ();
        }
    }

    protected abstract void OnRenderShape (DrawingContext context);
}

• Shape.BoundingBox is a Rectangle which defines the Shape bounds
• Shape.Children is an array of all inner shapes (in your sample I think you refer to it as InnerParts
• Shape.Transform is a transform which indicates how this Shape (and its children) relates to its parent
• OnRenderShape implementors implements this method to render the shape

The Transform class can express simple & complex transformations such as:

// Translates (Moves) Shape 100 "pixels" to right in relation to its parent
var translation = new TranslateTransform (100, 0);
// Rotates Shape 30 degrees clockwise in relation to its parent
var rotation = new RotateTransform (30);
// Composite first translates then rotates the Shape
var composite = 
            new TransformGroup
            {
                Children =
                    new TransformCollection
                            {
                                translation,
                                rotation ,
                            },
            }

So in order to express a simple composite object we can do it like this:

static RectangleShape MakeSimpleShape()
{
    return
        new RectangleShape
            {
                BoundingBox = new Rect (-200, -200, 400, 400),
                Pen = s_redPen,
                Brush = null,
                Children =
                    new Shape[]
                        {
                            new RectangleShape
                                {
                                    BoundingBox = new Rect (-40, -40, 40, 40),
                                    Transform = new TranslateTransform (100, 100),
                                },
                        },
            };
}

I did a full sample with rendering (using WPF) in case you are interested (MakeComplexShape basically builds a recursive shape up to a certain level)

• Create new Console Application in Visual Studio
• Add Reference to WindowsBase, PresentationCore, PresentationFramework, System.Xaml
• Paste code into Program.cs

Should be good to go.

using System;
using System.Linq;
using System.Windows;
using System.Windows.Media;
using System.Windows.Threading;

abstract class Shape
{
    static readonly Shape[] s_emptyChildren = new Shape[0];

    public Rect BoundingBox;
    public Transform Transform;
    public Shape[] Children = s_emptyChildren;

    public void RenderShape (DrawingContext context)
    {
        context.PushTransform (Transform ?? Transform.Identity);

        try
        {
            OnRenderShape (context);

            foreach (var shape in Children ?? s_emptyChildren)
            {
                shape.RenderShape (context);
            }
        }
        finally
        {
            context.Pop ();
        }
    }

    protected abstract void OnRenderShape (DrawingContext context);
}

sealed class RectangleShape : Shape
{
    static readonly SolidColorBrush s_defaultBrush = new SolidColorBrush (Colors.Green).FreezeIfNecessary ();

    public Pen Pen;
    public Brush Brush = s_defaultBrush;

    protected override void OnRenderShape (DrawingContext context)
    {
        context.DrawRectangle (Brush, Pen, BoundingBox);
    }
}

static class Extensions
{
    public static Color SetAlpha (this Color value, byte alpha)
    {
        return Color.FromArgb (alpha, value.R, value.G, value.B);
    }

    public static TValue FreezeIfNecessary<TValue>(this TValue value)
        where TValue : Freezable
    {
        if (value != null && value.CanFreeze)
        {
            value.Freeze ();
        }

        return value;
    }
}

class RenderShapeControl : FrameworkElement
{
    public Shape Shape;

    public Transform ShapeTransform;

    protected override void OnRender (DrawingContext drawingContext)
    {
        if (Shape != null)
        {
            try
            {
                drawingContext.PushTransform (new TranslateTransform (ActualWidth / 2, ActualHeight / 2).FreezeIfNecessary ());
                drawingContext.PushTransform (ShapeTransform ?? Transform.Identity);
                Shape.RenderShape (drawingContext);
            }
            finally
            {
                drawingContext.Pop ();
                drawingContext.Pop ();
            }
        }
    }

}

public class MainWindow : Window
{
    static readonly int[] s_childCount = new[] { 0, 5, 5, 5, 5, 5 };
    static readonly Brush s_redBrush = new SolidColorBrush (Colors.Red.SetAlpha (0x80)).FreezeIfNecessary ();
    static readonly Brush s_blueBrush = new SolidColorBrush (Colors.Blue.SetAlpha (0x80)).FreezeIfNecessary ();
    static readonly Pen s_redPen = new Pen (Brushes.Red, 2).FreezeIfNecessary ();
    static readonly Pen s_bluePen = new Pen (Brushes.Blue, 2).FreezeIfNecessary ();

    static Shape MakeInnerPart (int level, int index, int count, double outerside, double angle)
    {
        var innerSide = outerside / 3;
        return new RectangleShape
        {
            BoundingBox = new Rect (-innerSide / 2, -innerSide / 2, innerSide, innerSide),
            Pen = index == 0 ? s_bluePen : s_redPen,
            Brush = index == 0 && level > 0 ? s_redBrush : s_blueBrush,
            Children = MakeInnerParts (level - 1, innerSide),
            Transform =
                new TransformGroup
                {
                    Children =
                        new TransformCollection
                            {
                                new TranslateTransform (outerside/2, 0),
                                new RotateTransform (angle),
                            },
                }.FreezeIfNecessary (),
        };
    }

    static Shape[] MakeInnerParts (int level, double outerside)
    {
        var count = s_childCount[level];
        return Enumerable
            .Range (0, count)
            .Select (i => MakeInnerPart (level, i, count, outerside, (360.0 * i) / count))
            .ToArray ();
    }

    static RectangleShape MakeComplexShape ()
    {
        return new RectangleShape
        {
            BoundingBox = new Rect (-200, -200, 400, 400),
            Pen = s_redPen,
            Brush = null,
            Children = MakeInnerParts (3, 400),
        };
    }

    static RectangleShape MakeSimpleShape ()
    {
        return
            new RectangleShape
                {
                    BoundingBox = new Rect (-200, -200, 400, 400),
                    Pen = s_redPen,
                    Brush = null,
                    Children =
                        new Shape[]
                            {
                                new RectangleShape
                                    {
                                        BoundingBox = new Rect (-40, -40, 40, 40),
                                        Transform = new TranslateTransform (100, 100),
                                    },
                            },
                };
    }

    readonly DispatcherTimer m_dispatcher;
    readonly DateTime m_start = DateTime.Now;
    readonly RenderShapeControl m_shapeRenderer = new RenderShapeControl ();

    public MainWindow ()
    {
        AddChild (m_shapeRenderer);

        m_dispatcher = new DispatcherTimer (
            TimeSpan.FromSeconds (1 / 60),
            DispatcherPriority.ApplicationIdle,
            OnTimer,
            Dispatcher
            );
        m_dispatcher.Start ();
        m_shapeRenderer.Shape = MakeComplexShape ();
        //m_shapeRenderer.Shape = MakeSimpleShape ();
    }

    void OnTimer (object sender, EventArgs e)
    {
        var diff = DateTime.Now - m_start;
        var phase = (20 * diff.TotalSeconds) % 360.0;

        m_shapeRenderer.ShapeTransform =
            new TransformGroup
            {
                Children =
                    new TransformCollection
                        {
                            new TranslateTransform (100, 0),
                            new RotateTransform (phase),

                        },
            }.FreezeIfNecessary ();
        m_shapeRenderer.InvalidateVisual ();
    }
}

class Program
{
    [STAThread]
    static void Main (string[] args)
    {
        var mainWindow = new MainWindow ();
        mainWindow.ShowDialog ();
    }
}

The biggest "mess" is the way the angle is determined. You write that (hope you understand my syntax)

angle = angleof(part.XY relative part.baseXY) - angleof(this.baseXY relative this.XY)
angle = 2*pi - angle + angleof(this.XY relative this.baseXY)

The angle is not affected by the new coordinate... If you want a rotation around this.baseXY then I would try:

angle = angleof(part.XY relative part.baseXY) + angleof(this.XY relative this.baseXY) - angleof(new.XY relative this.baseXY)
part.baseXY = new.XY
part.XY = part.baseXY + radius * cos/sin(angle)

A good thing to test is that calling MoveParts with the same coordinates as current (this.MoveParts(this.X, this.Y) then nothing should happen...

(btw adding 2*pi to an angle makes no difference when calculating cos and sin)

And... if you are planning to do more advanced things, then look into another way of handling your coordinates, but thats another question...