WPF и Box2D. Как я делал физику c WPF

<Grid>
        <Viewport3D x:Name="viewport" ClipToBounds="True">
            <Viewport3D.Camera>
                <PerspectiveCamera FarPlaneDistance="1000" NearPlaneDistance="0.1" Position="0, 0, 10" LookDirection="0, 0, -1" UpDirection="0, 1, 0"/>
            </Viewport3D.Camera>
            <ModelVisual3D>
                <ModelVisual3D.Content>
                    <Model3DGroup x:Name="models">
						<AmbientLight Color="#333"/>
						<DirectionalLight Color="#FFF" Direction="-1, -1, -1"/>
						<DirectionalLight Color="#FFF" Direction="1, -1, -1"/>
					</Model3DGroup>
				</ModelVisual3D.Content>
			</ModelVisual3D>
		</Viewport3D>
    </Grid>

public class MyModel3D
	{
		public Vector3D Position { get; set; } // Позиция квадрата
		public Size Size { get; set; } // Размер квадрата
		private TranslateTransform3D translateTransform; // Матрица перемещения
		private RotateTransform3D rotationTransform; // Матрица вращения
		public MyModel3D(Model3DGroup models, double x, double y, double z, string path, Size size, float axis_x = 0, double angle = 0, float axis_y = 0, float axis_z = 1)
		{
			this.Size = size;
			this.Position = new Vector3D(x, y, z);
			MeshGeometry3D mesh = new MeshGeometry3D();
			// Проставляем вершины квадрату
			mesh.Positions = new Point3DCollection(new List<Point3D>
			{
				new Point3D(-size.Width/2, -size.Height/2, 0),
				new Point3D(size.Width/2, -size.Height/2, 0),
				new Point3D(size.Width/2, size.Height/2, 0),
				new Point3D(-size.Width/2, size.Height/2, 0)
			});
			// Указываем индексы для квадрата
			mesh.TriangleIndices = new Int32Collection(new List<int> { 0, 1, 2, 0, 2, 3 });
			mesh.TextureCoordinates = new PointCollection();
			// Устанавливаем текстурные координаты чтоб потом могли натянуть текстуру
			mesh.TextureCoordinates.Add(new Point(0, 1));
			mesh.TextureCoordinates.Add(new Point(1, 1));
			mesh.TextureCoordinates.Add(new Point(1, 0));
			mesh.TextureCoordinates.Add(new Point(0, 0));

			// Натягиваем текстуру
			ImageBrush brush = new ImageBrush(new BitmapImage(new Uri(path)));
			Material material = new DiffuseMaterial(brush);
			GeometryModel3D geometryModel = new GeometryModel3D(mesh, material);
			models.Children.Add(geometryModel);
			translateTransform = new TranslateTransform3D(x, y, z);
			rotationTransform = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(axis_x, axis_y, axis_z), angle), 0.5, 0.5, 0.5);

			Transform3DGroup tgroup = new Transform3DGroup();
			tgroup.Children.Add(translateTransform);
			tgroup.Children.Add(rotationTransform);
			geometryModel.Transform = tgroup;
		}
		// Утсанавливает позицию объекта
		public void SetPosition(Vector3D v3) 
		{
			translateTransform.OffsetX = v3.X;
			translateTransform.OffsetY = v3.Y;
			translateTransform.OffsetZ = v3.Z;
		}
		public Vector3D GetPosition()
		{
			return new Vector3D(translateTransform.OffsetX, translateTransform.OffsetY, translateTransform.OffsetZ);
		}
		// Поворачивает объект
		public void Rotation(Vector3D axis, double angle, double centerX = 0.5, double centerY = 0.5, double centerZ = 0.5)
		{
			rotationTransform.CenterX = translateTransform.OffsetX;
			rotationTransform.CenterY = translateTransform.OffsetY;
			rotationTransform.CenterZ = translateTransform.OffsetZ;

			rotationTransform.Rotation = new AxisAngleRotation3D(axis, angle);
		}
		public Size GetSize()
		{
			return Size;
		}
	}

public class Physics
{
	private World world;
	public Physics(float x, float y, float w, float h, float g_x, float g_y, bool doSleep)
	{
		AABB aabb = new AABB();
		aabb.LowerBound.Set(x, y); // Указываем левый верхний угол начала границ
		aabb.UpperBound.Set(w, h); // Указываем нижний правый угол конца границ
		Vec2 g = new Vec2(g_x, g_y); // Устанавливаеи вектор гравитации
		world = new World(aabb, g, doSleep); // Создаем мир
	}
}

public MyModel3D AddBox(float x, float y, float w, float h, float density, float friction, float restetution)
{
	// Создается наша графическая модель
	MyModel3D model = new MyModel3D(models, x, -y, 0, PATH_RECT, new System.Windows.Size(w, h));
	// Необходим для установи позиции, поворота, различных состояний и т.д. Советую поюзать свойства этих объектов
	BodyDef bDef = new BodyDef();
	bDef.Position.Set(x, y);
	bDef.Angle = 0;
	// Наш полигон который описывает вершины			
	PolygonDef pDef = new PolygonDef();
	pDef.Restitution = restetution;
	pDef.Friction = friction;
	pDef.Density = density;
	pDef.SetAsBox(w / 2, h / 2);
	// Создание самого тела
	Body body = world.CreateBody(bDef);
	body.CreateShape(pDef);
	body.SetMassFromShapes();
	body.SetUserData(model); // Это отличная функция, она на вход принемает объекты типа object, я ее использовал для того чтобы запихнуть и хранить в ней нашу графическую модель, и в методе step ее доставать и обновлять
	return model;
}

public MyModel3D AddCircle(float x, float y, float radius, float angle, float density,
	float friction, float restetution)
{
	MyModel3D model = new MyModel3D(models, x, -y, 0, PATH_CIRCLE, new System.Windows.Size(radius * 2, radius * 2));

	BodyDef bDef = new BodyDef();
	bDef.Position.Set(x, y);
	bDef.Angle = angle;

	CircleDef pDef = new CircleDef();
	pDef.Restitution = restetution;
	pDef.Friction = friction;
	pDef.Density = density;
	pDef.Radius = radius;

	Body body = world.CreateBody(bDef);
	body.CreateShape(pDef);
	body.SetMassFromShapes();
	body.SetUserData(model);
	return model;
}

public MyModel3D AddVert(float x, float y, Vec2[] vert, float angle, float density,
	float friction, float restetution)
{
	MyModel3D model = new MyModel3D(models, x, y, 0, Environment.CurrentDirectory + "\" + PATH_RECT, new System.Windows.Size(w, h)); // Данный метод нужно заменить на рисование многоугольников		

	BodyDef bDef = new BodyDef();
	bDef.Position.Set(x, y);
	bDef.Angle = angle;

	PolygonDef pDef = new PolygonDef();
	pDef.Restitution = restetution;
	pDef.Friction = friction;
	pDef.Density = density;
	pDef.SetAsBox(model.Size.Width / 2, model.Size.Height / 2);
	pDef.Vertices = vert;

	Body body = world.CreateBody(bDef);
	body.CreateShape(pDef);
	body.SetMassFromShapes();
	body.SetUserData(model);
	return info;
}

public void Step(float dt, int iterat)
{
	// Параметры этого метода управляют временем мира и точностью обработки коллизий тел
	world.Step(dt / 1000.0f, iterat, iterat);

	for (Body list = world.GetBodyList(); list != null; list = list.GetNext())
	{
		if (list.GetUserData() != null)
		{
			System.Windows.Media.Media3D.Vector3D position = new System.Windows.Media.Media3D.Vector3D(
			list.GetPosition().X, list.GetPosition().Y, 0);
			float angle = list.GetAngle() * 180.0f / (float)System.Math.PI; // Выполняем конвертацию из градусов в радианы
                        MyModel3D model = (MyModel3D)list.GetUserData();

		        model.SetPosition(position); // Перемещаем нашу графическую модель по x,y
			model.Rotation(new System.Windows.Media.Media3D.Vector3D(0, 0, 1), angle); // Вращаем по координате x
		}
	}
}

public partial class MainWindow : Window
{
	private Game.Physics px;
	public MainWindow()
	{
		InitializeComponent();

		px = new Game.Physics(-1000, -1000, 1000, 1000, 0, -0.005f, false);
		px.SetModelsGroup(models);
		px.AddBox(0.6f, -2, 1, 1, 0, 0.3f, 0.2f);
		px.AddBox(0, 0, 1, 1, 0.5f, 0.3f, 0.2f);

		this.LayoutUpdated += MainWindow_LayoutUpdated;
	}
	private void MainWindow_LayoutUpdated(object sender, EventArgs e)
	{
		px.Step(1.0f, 20); // тут по хорошему нужно вычислять дельту времени, но лень :)
		this.InvalidateArrange();
	}
}

public class Solver : ContactListener
	{
		public delegate void EventSolver(MyModel3D body1, MyModel3D body2);
		public event EventSolver OnAdd;
		public event EventSolver OnPersist;
		public event EventSolver OnResult;
		public event EventSolver OnRemove;

		public override void Add(ContactPoint point)
		{
			base.Add(point);

			OnAdd?.Invoke((MyModel3D)point.Shape1.GetBody().GetUserData(), (MyModel3D)point.Shape2.GetBody().GetUserData());
		}

		public override void Persist(ContactPoint point)
		{
			base.Persist(point);

			OnPersist?.Invoke((MyModel3D)point.Shape1.GetBody().GetUserData(), (MyModel3D)point.Shape2.GetBody().GetUserData());
		}

		public override void Result(ContactResult point)
		{
			base.Result(point);

			OnResult?.Invoke((MyModel3D)point.Shape1.GetBody().GetUserData(), (MyModel3D)point.Shape2.GetBody().GetUserData());
		}

		public override void Remove(ContactPoint point)
		{
			base.Remove(point);

			OnRemove?.Invoke((MyModel3D)point.Shape1.GetBody().GetUserData(), (MyModel3D)point.Shape2.GetBody().GetUserData());
		}
	}

public Joint AddJoint(Body b1, Body b2, float x, float y)
{
	RevoluteJointDef jd = new RevoluteJointDef();
	jd.Initialize(b1, b2, new Vec2(x, y));
	Joint joint = world.CreateJoint(jd);
			
	return joint;
}

public Joint AddDistanceJoint(Body b1, Body b2, float x1, float y1, float x2, float y2, 
	bool collideConnected = true, float hz = 1f)
{
	DistanceJointDef jd = new DistanceJointDef();
	jd.Initialize(b1, b2, new Vec2(x1, y1), new Vec2(x2, y2));
	jd.CollideConnected = collideConnected;
	jd.FrequencyHz = hz;

	Joint joint = world.CreateJoint(jd);

	return joint;
}