void DrawRobot(Graphics gr, PointF startPoint, Class2.Matrix4x4 projectionMatrix)
{
//проекция
var p = new Class2.Vector4[drRobot.Length];
for (int i = 0; i < drRobot.Length; i++)
{
p[i] = projectionMatrix * drRobot[i];
}
//создаем путь
var path = new GraphicsPath();
AddLine(path, p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9], p[10], p[11], p[12]);
//сдвигаем
gr.ResetTransform();
gr.TranslateTransform(startPoint.X, startPoint.Y);
//рисуем
Pen myWind = new Pen(Color.Blue, 5);
gr.DrawPath(myWind, path);
// gr.DrawLines(myWind, path.PathPoints);
gr.DrawEllipse(Pens.Black, -3 + (float)p[0].X, -3 + (float)p[0].Y, (float)7, (float)7);
gr.DrawEllipse(Pens.Black, -3 + (float)p[1].X, -3 + (float)p[1].Y, (float)7, (float)7);
gr.DrawEllipse(Pens.Black, -3 + (float)p[2].X, -3 + (float)p[2].Y, (float)7, (float)7);
gr.DrawEllipse(Pens.Black, -3 + (float)p[3].X, -3 + (float)p[3].Y, (float)7, (float)7);
gr.DrawEllipse(Pens.Black, -3 + (float)p[4].X, -3 + (float)p[4].Y, (float)7, (float)7);
gr.DrawEllipse(Pens.Black, -3 + (float)p[5].X, -3 + (float)p[5].Y, (float)7, (float)7);
gr.DrawEllipse(Pens.Black, -3 + (float)p[6].X, -3 + (float)p[6].Y, (float)7, (float)7);
}
private Class2.Vector4[] CreateCube(float scale, Class2.Vector4 position, float yaw, float pitch, float roll, ArmPosition position2)
{
//задаем координаты точек узлов робота
drRobot = new Class2.Vector4[14];
//Создаем список сочленений
float koef = 1.5F;
drRobot[0] = new Class2.Vector4((float)0 * koef, (float)0 * koef, (float)0 * koef, 1);
drRobot[1] = new Class2.Vector4((float)position2.X[0] * koef, (float)position2.Y[0] * koef, (float)position2.Z[0] * koef, 1);
drRobot[2] = new Class2.Vector4((float)position2.X[1] * koef, (float)position2.Y[1] * koef, (float)position2.Z[1] * koef, 1);
drRobot[3] = new Class2.Vector4((float)position2.X[2] * koef, (float)position2.Y[2] * koef, (float)position2.Z[2] * koef, 1);
drRobot[4] = new Class2.Vector4((float)position2.X[3] * koef, (float)position2.Y[3] * koef, (float)position2.Z[3] * koef, 1);
drRobot[5] = new Class2.Vector4((float)position2.X[4] * koef, (float)position2.Y[4] * koef, (float)position2.Z[4] * koef, 1);
drRobot[6] = new Class2.Vector4((float)position2.X[5] * koef, (float)position2.Y[5] * koef, (float)position2.Z[5] * koef, 1);
drRobot[7] = new Class2.Vector4((float)position2.X[6] * koef, (float)position2.Y[6] * koef, (float)position2.Z[6] * koef, 1);
drRobot[8] = new Class2.Vector4((float)position2.X[7] * koef, (float)position2.Y[7] * koef, (float)position2.Z[7] * koef, 1);
drRobot[9] = new Class2.Vector4((float)position2.X[8] * koef, (float)position2.Y[8] * koef, (float)position2.Z[8] * koef, 1);
drRobot[10] = new Class2.Vector4((float)position2.X[9] * koef, (float)position2.Y[9] * koef, (float)position2.Z[9] * koef, 1);
drRobot[11] = new Class2.Vector4((float)position2.X[10] * koef, (float)position2.Y[10] * koef, (float)position2.Z[10] * koef, 1);
drRobot[12] = new Class2.Vector4((float)position2.X[11] * koef, (float)position2.Y[11] * koef, (float)position2.Z[11] * koef, 1);
//матрица масштабирования
var scaleM = Class2.Matrix4x4.CreateScale(scale / 2);
//матрица вращения
var rotateM = Class2.Matrix4x4.CreateFromYawPitchRoll(yaw, pitch, roll);
//матрица переноса
var translateM = Class2.Matrix4x4.CreateTranslation(position);
//результирующая матрица
var m = translateM * rotateM * scaleM;
//умножаем векторы на матрицу
for (int i = 0; i < drRobot.Length; i++)
{
drRobot[i] = m * drRobot[i];
}
return(drRobot);
}