public Vector3 rotate(Common.Rotation rotation)
{
double tmp;
switch (rotation)
{
case Common.Rotation.ROTATION_NONE:
case Common.Rotation.ROTATION_MAX:
return this;
case Common.Rotation.ROTATION_YAW_45:
{
tmp = HALF_SQRT_2*(x - y);
y = HALF_SQRT_2*(x + y);
x = tmp;
return this;
}
case Common.Rotation.ROTATION_YAW_90:
{
tmp = x;
x = -y;
y = tmp;
return this;
}
case Common.Rotation.ROTATION_YAW_135:
{
tmp = -HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(x - y);
x = tmp;
return this;
}
case Common.Rotation.ROTATION_YAW_180:
x = -x;
y = -y;
return this;
case Common.Rotation.ROTATION_YAW_225:
{
tmp = HALF_SQRT_2*(y - x);
y = -HALF_SQRT_2*(x + y);
x = tmp;
return this;
}
case Common.Rotation.ROTATION_YAW_270:
{
tmp = x;
x = y;
y = -tmp;
return this;
}
case Common.Rotation.ROTATION_YAW_315:
{
tmp = HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(y - x);
x = tmp;
return this;
}
case Common.Rotation.ROTATION_ROLL_180:
{
y = -y;
z = -z;
return this;
}
case Common.Rotation.ROTATION_ROLL_180_YAW_45:
{
tmp = HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(x - y);
x = tmp;
z = -z;
return this;
}
case Common.Rotation.ROTATION_ROLL_180_YAW_90:
{
tmp = x;
x = y;
y = tmp;
z = -z;
return this;
}
case Common.Rotation.ROTATION_ROLL_180_YAW_135:
{
tmp = HALF_SQRT_2*(y - x);
y = HALF_SQRT_2*(y + x);
x = tmp;
z = -z;
return this;
}
case Common.Rotation.ROTATION_PITCH_180:
{
x = -x;
z = -z;
return this;
}
case Common.Rotation.ROTATION_ROLL_180_YAW_225:
{
tmp = -HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(y - x);
x = tmp;
z = -z;
return this;
}
case Common.Rotation.ROTATION_ROLL_180_YAW_270:
{
tmp = x;
x = -y;
y = -tmp;
z = -z;
return this;
}
case Common.Rotation.ROTATION_ROLL_180_YAW_315:
{
tmp = HALF_SQRT_2*(x - y);
y = -HALF_SQRT_2*(x + y);
x = tmp;
z = -z;
return this;
}
case Common.Rotation.ROTATION_ROLL_90:
{
tmp = z;
z = y;
y = -tmp;
return this;
}
case Common.Rotation.ROTATION_ROLL_90_YAW_45:
{
tmp = z;
z = y;
y = -tmp;
tmp = HALF_SQRT_2*(x - y);
y = HALF_SQRT_2*(x + y);
x = tmp;
return this;
}
case Common.Rotation.ROTATION_ROLL_90_YAW_90:
{
tmp = z;
z = y;
y = -tmp;
tmp = x;
x = -y;
y = tmp;
return this;
}
case Common.Rotation.ROTATION_ROLL_90_YAW_135:
{
tmp = z;
z = y;
y = -tmp;
tmp = -HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(x - y);
x = tmp;
return this;
}
case Common.Rotation.ROTATION_ROLL_270:
{
tmp = z;
z = -y;
y = tmp;
return this;
}
case Common.Rotation.ROTATION_ROLL_270_YAW_45:
{
tmp = z;
z = -y;
y = tmp;
tmp = HALF_SQRT_2*(x - y);
y = HALF_SQRT_2*(x + y);
x = tmp;
return this;
}
case Common.Rotation.ROTATION_ROLL_270_YAW_90:
{
tmp = z;
z = -y;
y = tmp;
tmp = x;
x = -y;
y = tmp;
return this;
}
case Common.Rotation.ROTATION_ROLL_270_YAW_135:
{
tmp = z;
z = -y;
y = tmp;
tmp = -HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(x - y);
x = tmp;
return this;
}
case Common.Rotation.ROTATION_PITCH_90:
{
tmp = z;
z = -x;
x = tmp;
return this;
}
case Common.Rotation.ROTATION_PITCH_270:
{
tmp = z;
z = x;
x = -tmp;
return this;
}
}
throw new Exception("Invalid Rotation");
//return this;
}
} private void Temp_Paint(object sender, PaintEventArgs e)
{
var rawdata = _dS.GetRaw();
e.Graphics.Clear(temp.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);
temp.Text = "Radius(+/-): " + (screenradius / 100.0) + "m MAV size([/]): " + (mavsize / 100.0) + "m";
// 11m radius = 22 m coverage
var scale = ((screenradius+50) * 2) / Math.Min(this.temp.Height,this.temp.Width);
// 80cm quad / scale
var size = mavsize / scale;
switch(_parent.cs.firmware)
{
case MainV2.Firmwares.ArduCopter2:
var imw = size/2;
e.Graphics.DrawImage(global::MissionPlanner.Properties.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(Control.DefaultFont.FontFamily, Control.DefaultFont.Size, FontStyle.Bold);
switch (temp.Orientation)
{
case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_NONE:
location.rotate(Common.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(Common.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(Common.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(Common.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(Common.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(Common.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(Common.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(Common.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;
}
}
}
