public void AimFor(MissionPlanner.Utilities.Vector3 point) { lock (aimpoints) { //point.Normalize(); //point *= scale; aimpoints.Add(new Vector3((float)point.x, (float)point.y, (float)point.z)); } }
public void AddPoint(MissionPlanner.Utilities.Vector3 point) { minx = (float)Math.Min(minx, point.X); maxx = (float)Math.Max(maxx, point.X); miny = (float)Math.Min(miny, point.Y); maxy = (float)Math.Max(maxy, point.Y); minz = (float)Math.Min(minz, point.Z); maxz = (float)Math.Max(maxz, point.Z); lock (points) { //point.Normalize(); //point *= scale; points.Add(new Vector3((float)point.x, (float)point.y, (float)point.z)); } this.Invalidate(); }
protected override void OnPaint(System.Windows.Forms.PaintEventArgs e) { if (this.DesignMode) { e.Graphics.Clear(Color.Black); return; } GL.Viewport(0, 0, this.Width, this.Height); // radians += 5 * deg2rad; if (rotatewithdata) { yaw += 5 * deg2rad; } MakeCurrent(); GL.MatrixMode(MatrixMode.Projection); double max = Math.Max(Math.Max((maxx - minx) / 2, (maxy - miny) / 2), (maxz - minz) / 2); if (max < 300) { max = 400; } max *= 1.3; if (points.Count > 0) { //Vector3 current = new Vector3(points[points.Count - 1].X, points[points.Count - 1].Y, points[points.Count - 1].Z); //yaw = Math.Atan2(points[points.Count - 1].X, points[points.Count - 1].Y); } OpenTK.Matrix4 projection = OpenTK.Matrix4.CreatePerspectiveFieldOfView((float)(45 * deg2rad), 1f, 0.00001f, 5000.0f); GL.LoadMatrix(ref projection); float eyedist = (float)max * 3; // Z X Y eye = Vector3.TransformPosition(eye, Matrix4.CreateRotationZ((float)yaw)); yaw = 0; pitch = 0; if (float.IsNaN(eye.X)) { eye = new Vector3(1, 1, 1); } eye.Normalize(); eye *= eyedist; //Console.WriteLine("eye "+ eye.ToString()); //(maxx + minx) / 2, (maxy + miny) / 2, (maxz + minz) / 2 Matrix4 modelview = Matrix4.LookAt(eye.X, eye.Y, eye.Z, 0, 0, 0, 0, 0, 1); // CenterPoint.X, CenterPoint.Y, CenterPoint.Z GL.MatrixMode(MatrixMode.Modelview); GL.LoadMatrix(ref modelview); GL.ClearColor(Color.Black); GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit); // 3 GL.PointSize(8); GL.Begin(PrimitiveType.Lines); // +tivs GL.Color3(Color.FromArgb(0, 0, 255)); GL.Vertex3(0, 0, 0); GL.Vertex3(0, 0, max); GL.Color3(Color.FromArgb(0, 255, 0)); GL.Vertex3(0, 0, 0); GL.Vertex3(0, max, 0); GL.Color3(Color.FromArgb(255, 0, 0)); GL.Vertex3(0, 0, 0); GL.Vertex3(max, 0, 0); // -atives GL.Color3(Color.FromArgb(255, 255, 0)); GL.Vertex3(0, 0, 0); GL.Vertex3(0, 0, -max); GL.Color3(Color.FromArgb(255, 0, 255)); GL.Vertex3(0, 0, 0); GL.Vertex3(0, -max, 0); GL.Color3(Color.FromArgb(0, 255, 255)); GL.Vertex3(0, 0, 0); GL.Vertex3(-max, 0, 0); GL.End(); //GL.Rotate(Pitch, 0, 0, 0); //GL.Rotate(Roll, 0, 0, 0); // GL.Rotate(Yaw, 0, 0, 0); GL.Begin(PrimitiveType.Points); lock (points) { foreach (var item in points) { float rangex = maxx - minx; float rangey = maxy - miny; float rangez = maxz - minz; int valuex = (int)Math.Abs((((item.X) / rangex) * 254)) & 0xff; int valuey = (int)Math.Abs((((item.Y) / rangey) * 254)) & 0xff; int valuez = (int)Math.Abs((((item.Z) / rangez) * 254)) & 0xff; Color col = Color.FromArgb(valuex, valuey, valuez); GL.Color3(col); var vec = new MissionPlanner.Utilities.Vector3(item.X, item.Y, item.Z) + CenterPoint; GL.Vertex3(new Vector3((float)vec.x, (float)vec.y, (float)vec.z)); } lock (aimpoints) { foreach (var aim in aimpoints) { GL.PointSize(8); GL.Color3(Color.White); var vec = new MissionPlanner.Utilities.Vector3(aim.X, aim.Y, aim.Z) + CenterPoint; GL.Vertex3(new Vector3((float)vec.x, (float)vec.y, (float)vec.z)); } } GL.End(); // 8 GL.PointSize(12); GL.Begin(PrimitiveType.Points); GL.Color3(Color.Red); if (points.Count > 0) { var vec = new MissionPlanner.Utilities.Vector3(points[points.Count - 1].X, points[points.Count - 1].Y, points[points.Count - 1].Z) + CenterPoint; GL.Vertex3(new Vector3((float)vec.x, (float)vec.y, (float)vec.z)); } } GL.End(); Console.WriteLine(Math.Atan2(eye.Y, eye.X)); //float newyaw = 0 * deg2rad; //DrawCircle(CenterPoint.X, CenterPoint.Y, CenterPoint.Z, newyaw, (float)(max), 60); this.SwapBuffers(); }