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