public void Draw() { GLUtility.GoToTransform((float)location.x, (float)location.y); GLUtility.GoToTransform((float)location.yaw); GLUtility.FillCone(renderColor, opacity, new PointF(0, 0), (float)range, (float)-fov / 2.0f, (float)fov / 2.0f); GLUtility.ComeBackFromTransform(); GLUtility.ComeBackFromTransform(); }
public void Draw(Renderer cam) { if (poses.Count == 0) { return; } lock (poses) { RobotPose lastPose = poses[0]; foreach (RobotPose p in poses) { GLUtility.DrawCross(pen, p.ToVector2(), .2f); //GLUtility.DrawLine(pen, lastPose.ToVector2(), p.ToVector2()); lastPose = p; } if (poses[poses.Count - 1].covariance[0, 0] != 0) { //calculate the error ellipse Matrix cov = new Matrix(2, 2); cov[0, 0] = poses[poses.Count - 1].covariance[0, 0]; cov[0, 1] = poses[poses.Count - 1].covariance[0, 1]; cov[1, 0] = poses[poses.Count - 1].covariance[1, 0]; cov[1, 1] = poses[poses.Count - 1].covariance[1, 1]; double theta = .5 * Math.Atan2((-2 * cov[0, 1]), (cov[0, 0] - cov[1, 1])); if (Double.IsNaN(theta)) { theta = 0; } double sigu2 = (cov[0, 0] * Math.Sin(theta) * Math.Sin(theta)) + (2 * cov[0, 1] * Math.Sin(theta) * Math.Cos(theta)) + (cov[1, 1] * Math.Cos(theta) * Math.Cos(theta)); double sigv2 = (cov[0, 0] * Math.Cos(theta) * Math.Cos(theta)) + (2 * cov[0, 1] * Math.Sin(theta) * Math.Cos(theta)) + (cov[1, 1] * Math.Sin(theta) * Math.Sin(theta)); GLUtility.GoToTransformXYZ((float)poses[poses.Count - 1].x, (float)poses[poses.Count - 1].y, 0); GLUtility.GoToTransformYPR((float)theta, 0, 0); GLUtility.DrawEllipse(ellipsePen, new RectangleF((float)-sigu2 / 2, (float)-sigv2 / 2, (float)sigu2, (float)sigv2)); GLUtility.ComeBackFromTransform(); GLUtility.ComeBackFromTransform(); } } }