public void normalize() { // '''re-normalise a rotation matrix''' Vector3 error = a * b; Vector3 t0 = a - (b * (0.5 * error)); Vector3 t1 = b - (a * (0.5 * error)); Vector3 t2 = t0 % t1; a = t0 * (1.0 / t0.length()); b = t1 * (1.0 / t1.length()); c = t2 * (1.0 / t2.length()); }
private void Temp_Paint(object sender, PaintEventArgs e) { var rawdata = _dS.GetRaw(); e.Graphics.Clear(BackColor); var midx = e.ClipRectangle.Width / 2.0f; var midy = e.ClipRectangle.Height / 2.0f; //e.Graphics.DrawArc(System.Drawing.Pens.Green, midx - 10, midy - 10, 20, 20, 0, 360); Text = "Radius(+/-): " + (screenradius / 100.0) + "m MAV size([/]): " + (mavsize / 100.0) + "m"; // 11m radius = 22 m coverage var scale = ((screenradius + 50) * 2) / Math.Min(Height, Width); // 80cm quad / scale var size = mavsize / scale; switch (_parent.cs.firmware) { case Firmwares.ArduCopter2: var imw = size / 2; e.Graphics.DrawImage(Resources.quadicon, midx - imw, midy - imw, size, size); break; } foreach (var temp in rawdata.ToList()) { Vector3 location = new Vector3(0, Math.Min(temp.Distance / scale, (screenradius) / scale), 0); var halflength = location.length() / 2.0f; var doublelength = location.length() * 2.0f; var length = location.length(); Pen redpen = new Pen(Color.Red, 3); float move = 5; var font = new Font(SystemFonts.DefaultFont.FontFamily, SystemFonts.DefaultFont.Size + 2, FontStyle.Bold); switch (temp.Orientation) { case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_NONE: location.rotate(Rotation.ROTATION_NONE); e.Graphics.DrawString((temp.Distance / 100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x - move * 2, midy - (float)location.y + move); e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, -22.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_45: location.rotate(Rotation.ROTATION_YAW_45); e.Graphics.DrawString((temp.Distance / 100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x - move * 8, midy - (float)location.y + move); e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 22.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_90: location.rotate(Rotation.ROTATION_YAW_90); e.Graphics.DrawString((temp.Distance / 100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x - move * 8, midy - (float)location.y); e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 67.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_135: location.rotate(Rotation.ROTATION_YAW_135); e.Graphics.DrawString((temp.Distance / 100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x - move * 8, midy - (float)location.y - move); e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 112.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_180: location.rotate(Rotation.ROTATION_YAW_180); e.Graphics.DrawString((temp.Distance / 100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x - move * 2, midy - (float)location.y - move * 3); e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 157.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_225: location.rotate(Rotation.ROTATION_YAW_225); e.Graphics.DrawString((temp.Distance / 100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x + move, midy - (float)location.y - move); e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 202.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_270: location.rotate(Rotation.ROTATION_YAW_270); e.Graphics.DrawString((temp.Distance / 100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x + move, midy - (float)location.y); e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 247.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_315: location.rotate(Rotation.ROTATION_YAW_315); e.Graphics.DrawString((temp.Distance / 100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x + move, midy - (float)location.y); e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 292.5f - 90f, 45f); break; } } }
private void Temp_Paint(object sender, PaintEventArgs e) { var rawdata = _dS.GetRaw(); e.Graphics.Clear(temp.BackColor); var midx = this.temp.Width / 2.0f; var midy = this.temp.Height / 2.0f; e.Graphics.DrawArc(System.Drawing.Pens.Green, midx - 10, midy - 10, 20, 20, 0, 360); foreach (var temp in rawdata.ToList()) { Vector3 location = new Vector3(0, temp.Distance / 5, 0); var halflength = location.length() / 2.0f; var doublelength = location.length() * 2.0f; var length = location.length(); switch (temp.Orientation) { case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_NONE: location.rotate(Common.Rotation.ROTATION_NONE); e.Graphics.DrawString(temp.Distance.ToString(), Control.DefaultFont, System.Drawing.Brushes.Green, midx - (float)location.x, midy - (float)location.y); e.Graphics.DrawArc(System.Drawing.Pens.Red, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, -22.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_45: location.rotate(Common.Rotation.ROTATION_YAW_45); e.Graphics.DrawString(temp.Distance.ToString(), Control.DefaultFont, System.Drawing.Brushes.Green, midx - (float)location.x, midy - (float)location.y); e.Graphics.DrawArc(System.Drawing.Pens.Red, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 22.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_90: location.rotate(Common.Rotation.ROTATION_YAW_90); e.Graphics.DrawString(temp.Distance.ToString(), Control.DefaultFont, System.Drawing.Brushes.Green, midx - (float)location.x, midy - (float)location.y); e.Graphics.DrawArc(System.Drawing.Pens.Red, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 67.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_135: location.rotate(Common.Rotation.ROTATION_YAW_135); e.Graphics.DrawString(temp.Distance.ToString(), Control.DefaultFont, System.Drawing.Brushes.Green, midx - (float)location.x, midy - (float)location.y); e.Graphics.DrawArc(System.Drawing.Pens.Red, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 112.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_180: location.rotate(Common.Rotation.ROTATION_YAW_180); e.Graphics.DrawString(temp.Distance.ToString(), Control.DefaultFont, System.Drawing.Brushes.Green, midx - (float)location.x, midy - (float)location.y); e.Graphics.DrawArc(System.Drawing.Pens.Red, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 157.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_225: location.rotate(Common.Rotation.ROTATION_YAW_225); e.Graphics.DrawString(temp.Distance.ToString(), Control.DefaultFont, System.Drawing.Brushes.Green, midx - (float)location.x, midy - (float)location.y); e.Graphics.DrawArc(System.Drawing.Pens.Red, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 202.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_270: location.rotate(Common.Rotation.ROTATION_YAW_270); e.Graphics.DrawString(temp.Distance.ToString(), Control.DefaultFont, System.Drawing.Brushes.Green, midx - (float)location.x, midy - (float)location.y); e.Graphics.DrawArc(System.Drawing.Pens.Red, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 247.5f - 90f, 45f); break; case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_315: location.rotate(Common.Rotation.ROTATION_YAW_315); e.Graphics.DrawString(temp.Distance.ToString(), Control.DefaultFont, System.Drawing.Brushes.Green, midx - (float)location.x, midy - (float)location.y); e.Graphics.DrawArc(System.Drawing.Pens.Red, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 292.5f - 90f, 45f); break; } } }