Point3D right_foot = transformator.TransformPoint3D(new Point3D(body.Joints[JointType.FootRight].Position.X, body.Joints[JointType.FootRight].Position.Y, body.Joints[JointType.FootRight].Position.Z));