public static Quaternion NgimuToUnityQuaternion(NgimuApi.Maths.Quaternion quaternion)
{
NgimuApi.Maths.Quaternion ngimuQuaternion = NgimuApi.Maths.Quaternion.Normalise(NgimuApi.Maths.Quaternion.Conjugate(quaternion));
Matrix4x4 matrix = Matrix4x4.TRS(Vector3.zero, new Quaternion(ngimuQuaternion.X, ngimuQuaternion.Y, ngimuQuaternion.Z, ngimuQuaternion.W), Vector3.one);
Vector4 yColumn = matrix.GetColumn(1);
Vector4 zColumn = matrix.GetColumn(2);
matrix.SetColumn(1, zColumn);
matrix.SetColumn(2, yColumn);
Vector4 yRow = matrix.GetRow(1);
Vector4 zRow = matrix.GetRow(2);
matrix.SetRow(1, zRow);
matrix.SetRow(2, yRow);
return(Quaternion.LookRotation(matrix.GetColumn(2), matrix.GetColumn(1)));
}
/// <summary>
/// Redraw cuboid polygons.
/// </summary>
private void GlControl1_Paint(object sender, PaintEventArgs e)
{
if (isInErrorState == true)
{
return;
}
try
{
if (WindowState == FormWindowState.Minimized)
{
return;
}
EulerAngles euler = NgimuApi.Maths.Quaternion.ToEulerAngles(quaternion);
NgimuApi.Maths.Quaternion quat = quaternion;
// Apply transformation matrix to cuboid
RotationMatrix rotationMatrix = NgimuApi.Maths.Quaternion.ToRotationMatrix(quat);
try
{
glControl1.MakeCurrent();
}
catch (OpenTK.Graphics.GraphicsContextException ex)
{
DisposeOpenGLControl();
LoadOpenGLControl();
return;
}
view.Rotation = OpenTK.Quaternion.FromAxisAngle(OpenTK.Vector3.UnitY, m_ViewYaw) * OpenTK.Quaternion.FromAxisAngle(OpenTK.Vector3.UnitX, m_ViewPitch);
view.UpdateProjection();
foreach (ModelSceneLight sceneLight in sceneLights)
{
sceneLight.Update();
}
GLState.ClearColor(new Color4(15, 15, 15, 255));
GLState.CullFace(OpenTK.Graphics.OpenGL.CullFaceMode.Back);
GLState.EnableCullFace = true;
GLState.EnableDepthMask = true;
GLState.EnableDepthTest = true;
GLState.EnableBlend = true;
GLState.BlendFunc(BlendingFactorSrc.SrcAlpha, BlendingFactorDest.OneMinusSrcAlpha);
GLState.BlendEquation(BlendEquationMode.FuncAdd);
GLState.ClearDepth(1.0f);
GLState.Viewport = view.Viewport;
GLState.ApplyAll(glControl1.Size);
GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);
Matrix4 model = Matrix4.Identity;
OpenTK.Quaternion inverse = OpenTK.Quaternion.Invert(new OpenTK.Quaternion(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W));
model *= Matrix4.CreateScale(1f / modelScale);
model *= Matrix4.CreateScale(3f);
model *= Matrix4.CreateFromQuaternion(inverse);
model *= Matrix4.CreateRotationX(-MathHelper.PiOver2);
OpenTK.Quaternion normalQuaternion = model.ExtractRotation();
Matrix4 normal = Matrix4.CreateFromQuaternion(normalQuaternion);
float Emissivity = 0f;
float Alpha = 1f;
OpenTK.Vector3 color = new OpenTK.Vector3(1f, 1f, 1f);
OpenTK.Vector3 modelColor = new OpenTK.Vector3(0.6f, 0.6f, 0.6f);
Vector2 surface = new Vector2(1f - (Emissivity * Emissivity), Alpha);
lightingArguments.NormalWorldMatrix = view.NormalWorld;
lightingArguments.WorldMatrix = view.World;
lightingArguments.ProjectionMatrix = view.Projection;
for (int i = 0; i < sceneLights.Length; i++)
{
lights[i] = sceneLights[i].ToLightInstance(ref view.World, ref view.NormalWorld);
}
ModelLightInstance light = lights[0];
if (imuModelToolStripMenuItem.Checked == true)
{
material.Render(ref model, ref normal, ref modelColor, ref surface, null, lightingArguments, ref light, imuModel.Vertices);
}
if (earthToolStripMenuItem.Checked == true)
{
Matrix4 tileModel = Matrix4.Identity;
Matrix4 tileModelNormal = Matrix4.Identity;
tileModel *= Matrix4.CreateScale(4f, 0.1f, 4f);
tileModel *= Matrix4.CreateTranslation(new OpenTK.Vector3(0, -3.5f, 0));
tileModel *= Matrix4.CreateScale(1f / modelScale);
color = new OpenTK.Vector3(1f, 1f, 1f);
surface = new Vector2(1f, 0.5f);
tileMaterial.Render(ref tileModel, ref tileModelNormal, ref color, ref surface, tileTexture, lightingArguments, ref light, floorTileModel.Vertices);
//material.Render(ref tileModel, ref tileModelNormal, ref color, ref surface, null, lightingArguments, ref light, floorTileModel.Vertices);
}
textRenderer.Clear(Color.Transparent);
if (imuAxesToolStripMenuItem.Checked == true)
{
RenderAxes(model, OpenTK.Vector3.Zero, inverse, normalQuaternion, normal, Emissivity, Alpha, color, surface, light, 1f, 3.5f);
Matrix4 earthMatrix = Matrix4.Identity;
OpenTK.Vector3 offset = UnProject(ref view.Projection, ref view.View, glControl1.Size, new Vector2(-0.75f, -0.75f));
//OpenTK.Vector3 offset = new OpenTK.Vector3(0.5f, 0.5f, 0.1f);
OpenTK.Quaternion earthInverse = OpenTK.Quaternion.Identity;
OpenTK.Quaternion earthNormal = OpenTK.Quaternion.Identity;
RenderAxes(earthMatrix, offset, earthInverse, earthNormal, earthMatrix, Emissivity, Alpha, color, surface, light, 0.025f, 3.75f);
}
if (eulerAnglesToolStripMenuItem.Checked == true)
{
textRenderer.DrawString("Roll", m_BaseFont2, Brushes.LightGray, new PointF(20, 44), false);
textRenderer.DrawString("Pitch", m_BaseFont2, Brushes.LightGray, new PointF(20, 84), false);
textRenderer.DrawString("Yaw", m_BaseFont2, Brushes.LightGray, new PointF(20, 124), false);
int verticleOffset = 4;
textRenderer.DrawString(String.Format("{0,7:###0.0}", euler[0]) + "°", m_BaseFont, Brushes.LightGray, new PointF(80, 44 + verticleOffset), false);
textRenderer.DrawString(String.Format("{0,7:###0.0}", euler[1]) + "°", m_BaseFont, Brushes.LightGray, new PointF(80, 84 + verticleOffset), false);
textRenderer.DrawString(String.Format("{0,7:###0.0}", euler[2]) + "°", m_BaseFont, Brushes.LightGray, new PointF(80, 124 + verticleOffset), false);
}
textRenderer.Update();
texturedQuad.Render(view);
glControl1.SwapBuffers();
}
catch (Exception ex)
{
using (ExceptionDialog dialog = new ExceptionDialog())
{
dialog.Title = "An Exception Occurred";
dialog.Label = ex.Message;
dialog.Detail = ex.ToString();
dialog.ShowDialog(this);
}
isInErrorState = true;
}
}
