public void AddDataSetToDataholder(int x, int y, int z, int yaw, int m1, int m2, int m3, int m4, int m5)
{
Vector3 midPos = (new Vector3(x, z, y)) * 0.001f;
Vector3 m1Point = Vector3.zero;
float cos = Mathf.Cos((yaw + 90f) * (Mathf.PI / 180f));
float sin = Mathf.Sin((yaw + 90f) * (Mathf.PI / 180f));
m1Point = (new Vector3(sin, 0f, -cos)) * m1 * 0.001f;
m1Point += midPos;
Vector3 m2Point = Vector3.zero;
cos = Mathf.Cos((yaw - 90f) * (Mathf.PI / 180f));
sin = Mathf.Sin((yaw - 90f) * (Mathf.PI / 180f));
m2Point = (new Vector3(sin, 0f, -cos)) * m2 * 0.001f;
m2Point += midPos;
Vector3 m3Point = Vector3.zero;
m3Point = (new Vector3(0f, 1f, 0f)) * m3 * 0.001f;
m3Point += midPos;
Vector3 m4Point = Vector3.zero;
cos = Mathf.Cos((yaw + 180f) * (Mathf.PI / 180f));
sin = Mathf.Sin((yaw + 180f) * (Mathf.PI / 180f));
m4Point = (new Vector3(sin, 0f, -cos)) * m4 * 0.001f;
m4Point += midPos;
Vector3 m5Point = Vector3.zero;
cos = Mathf.Cos((yaw) * (Mathf.PI / 180f));
sin = Mathf.Sin((yaw) * (Mathf.PI / 180f));
m5Point = (new Vector3(sin, 0f, -cos)) * m5 * 0.001f;
m5Point += midPos;
dataHolder.AddData(m1Point, midPos, yaw, 1);
dataHolder.AddData(m2Point, midPos, yaw, 1);
//dataHolder.AddData(m3Point, midPos, yaw, Color.blue);
dataHolder.AddData(m4Point, midPos, yaw, 1);
dataHolder.AddData(m5Point, midPos, yaw, 1);
dataHolder.AddData(midPos, midPos, yaw, 0);
}