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;
}
}
}
