public void RotateLocalZ(float _Radians)
{
CMatrix4x4 rotMatrix = new CMatrix4x4();
rotMatrix = rotMatrix.MakeRotationZ(_Radians);
m_Matrix = Multiply(rotMatrix.m_Matrix, m_Matrix);
}
public CMatrix4x4 MakeRotationZ(float _Radians)
{
float sinAngle = (float)Math.Sin(_Radians);
float cosAngle = (float)Math.Cos(_Radians);
CMatrix4x4 newMatrix = new CMatrix4x4();
SetIndex(0, 0, cosAngle);
SetIndex(0, 1, sinAngle);
SetIndex(1, 0, -sinAngle);
SetIndex(1, 1, cosAngle);
return(newMatrix);
}
public CMatrix4x4 Multiply(CMatrix4x4 _First, CMatrix4x4 _Second)
{
CMatrix4x4 newMatrix = new CMatrix4x4();
float[,] first = _First.m_Matrix;
float[,] second = _Second.m_Matrix;
// X Axis
newMatrix.m_Matrix[0, 0] = first[0, 0] * second[0, 0] +
first[0, 1] * second[1, 0] +
first[0, 2] * second[2, 0] +
first[0, 3] * second[3, 0];
newMatrix.m_Matrix[0, 1] = first[0, 0] * second[0, 1] +
first[0, 1] * second[1, 1] +
first[0, 2] * second[2, 1] +
first[0, 3] * second[3, 1];
newMatrix.m_Matrix[0, 2] = first[0, 0] * second[0, 2] +
first[0, 1] * second[1, 2] +
first[0, 2] * second[2, 2] +
first[0, 3] * second[3, 2];
newMatrix.m_Matrix[0, 3] = first[0, 0] * second[0, 3] +
first[0, 1] * second[1, 3] +
first[0, 2] * second[2, 3] +
first[0, 3] * second[3, 3];
// Y Axis
newMatrix.m_Matrix[1, 0] = first[1, 0] * second[0, 0] +
first[1, 1] * second[1, 0] +
first[1, 2] * second[2, 0] +
first[1, 3] * second[3, 0];
newMatrix.m_Matrix[1, 1] = first[1, 0] * second[0, 1] +
first[1, 1] * second[1, 1] +
first[1, 2] * second[2, 1] +
first[1, 3] * second[3, 1];
newMatrix.m_Matrix[1, 2] = first[1, 0] * second[0, 2] +
first[1, 1] * second[1, 2] +
first[1, 2] * second[2, 2] +
first[1, 3] * second[3, 2];
newMatrix.m_Matrix[1, 3] = first[1, 0] * second[0, 3] +
first[1, 1] * second[1, 3] +
first[1, 2] * second[2, 3] +
first[1, 3] * second[3, 3];
// Z Axis
newMatrix.m_Matrix[2, 0] = first[2, 0] * second[0, 0] +
first[2, 1] * second[1, 0] +
first[2, 2] * second[2, 0] +
first[2, 3] * second[3, 0];
newMatrix.m_Matrix[2, 1] = first[2, 0] * second[0, 1] +
first[2, 1] * second[1, 1] +
first[2, 2] * second[2, 1] +
first[2, 3] * second[3, 1];
newMatrix.m_Matrix[2, 2] = first[2, 0] * second[0, 2] +
first[2, 1] * second[1, 2] +
first[2, 2] * second[2, 2] +
first[2, 3] * second[3, 2];
newMatrix.m_Matrix[2, 3] = first[2, 0] * second[0, 3] +
first[2, 1] * second[1, 3] +
first[2, 2] * second[2, 3] +
first[2, 3] * second[3, 3];
// W Axis
newMatrix.m_Matrix[3, 0] = first[3, 0] * second[0, 0] +
first[3, 1] * second[1, 0] +
first[3, 2] * second[2, 0] +
first[3, 3] * second[3, 0];
newMatrix.m_Matrix[3, 1] = first[3, 0] * second[0, 1] +
first[3, 1] * second[1, 1] +
first[3, 2] * second[2, 1] +
first[3, 3] * second[3, 1];
newMatrix.m_Matrix[3, 2] = first[3, 0] * second[0, 2] +
first[3, 1] * second[1, 2] +
first[3, 2] * second[2, 2] +
first[3, 3] * second[3, 2];
newMatrix.m_Matrix[3, 3] = first[3, 0] * second[0, 3] +
first[3, 1] * second[1, 3] +
first[3, 2] * second[2, 3] +
first[3, 3] * second[3, 3];
return(newMatrix);
}
private void OpenModelXML(string _Path)
{
XElement root = XElement.Load(_Path);
// Pull the model part
XElement modelpart = root.Element("GDModelPart");
// Pull the parent index
ParentNum.Value = Convert.ToInt32(modelpart.Element("ParentIndex").Value);
// Pull the matrix
CMatrix4x4 matrix = new CMatrix4x4();
XElement transform = modelpart.Element("LocalTransform");
// Pull the X Axis
XElement xaxis = transform.Element("XAxis");
matrix.SetIndex(0, 0, (float)Convert.ToDouble(xaxis.Element("x").Value));
matrix.SetIndex(0, 1, (float)Convert.ToDouble(xaxis.Element("y").Value));
matrix.SetIndex(0, 2, (float)Convert.ToDouble(xaxis.Element("z").Value));
matrix.SetIndex(0, 3, (float)Convert.ToDouble(xaxis.Element("w").Value));
// Pull the Y Axis
XElement yaxis = transform.Element("YAxis");
matrix.SetIndex(1, 0, (float)Convert.ToDouble(yaxis.Element("x").Value));
matrix.SetIndex(1, 1, (float)Convert.ToDouble(yaxis.Element("y").Value));
matrix.SetIndex(1, 2, (float)Convert.ToDouble(yaxis.Element("z").Value));
matrix.SetIndex(1, 3, (float)Convert.ToDouble(yaxis.Element("w").Value));
// Pull the Z Axis
XElement zaxis = transform.Element("ZAxis");
matrix.SetIndex(2, 0, (float)Convert.ToDouble(zaxis.Element("x").Value));
matrix.SetIndex(2, 1, (float)Convert.ToDouble(zaxis.Element("y").Value));
matrix.SetIndex(2, 2, (float)Convert.ToDouble(zaxis.Element("z").Value));
matrix.SetIndex(2, 3, (float)Convert.ToDouble(zaxis.Element("w").Value));
// Pull the W Axis
XElement waxis = transform.Element("WAxis");
matrix.SetIndex(3, 0, (float)Convert.ToDouble(waxis.Element("x").Value));
matrix.SetIndex(3, 1, (float)Convert.ToDouble(waxis.Element("y").Value));
matrix.SetIndex(3, 2, (float)Convert.ToDouble(waxis.Element("z").Value));
matrix.SetIndex(3, 3, (float)Convert.ToDouble(waxis.Element("w").Value));
float y1, y2, x1, x2, z1, z2;
// Convert back to radians
if (matrix.GetIndex(3, 1) != 1 && matrix.GetIndex(3, 1) != -1)
{
y1 = (float)-Math.Asin(matrix.GetIndex(3, 1));
y2 = (float)Math.PI - y1;
float cosy1 = (float)Math.Cos(y1);
float cosy2 = (float)Math.Cos(y2);
x1 = (float)Math.Atan2(matrix.GetIndex(3, 2) / cosy1, matrix.GetIndex(3, 3) / cosy1);
x2 = (float)Math.Atan2(matrix.GetIndex(3, 2) / cosy2, matrix.GetIndex(3, 3) / cosy2);
z1 = (float)Math.Atan2(matrix.GetIndex(2, 1) / cosy1, matrix.GetIndex(1, 1) / cosy1);
z2 = (float)Math.Atan2(matrix.GetIndex(2, 1) / cosy2, matrix.GetIndex(1, 1) / cosy2);
}
else
{
z2 = 0.0f;
if (matrix.GetIndex(3, 1) == -1)
{
y2 = (float)Math.PI / 2.0f;
x2 = (float)Math.Atan2(matrix.GetIndex(1, 2), matrix.GetIndex(1, 3));
}
else
{
y2 = (float)-Math.PI / 2.0f;
x2 = (float)Math.Atan2(-matrix.GetIndex(1, 2), -matrix.GetIndex(1, 3));
}
}
if (radiansToolStripMenuItem.Checked)
{
RotXNum.Value = (decimal)x2;
RotYNum.Value = (decimal)y2;
RotZNum.Value = (decimal)z2;
}
else
{
RotXNum.Value = (decimal)(x2 * 180.0f / (float)Math.PI);
RotYNum.Value = (decimal)(y2 * 180.0f / (float)Math.PI);
RotZNum.Value = (decimal)(z2 * 180.0f / (float)Math.PI);
}
}
// Save out model file
private void SaveModelXML(string _Filename, string _Path)
{
XElement root = new XElement("GDModel");
XElement modelpart = new XElement("GDModelPart");
root.Add(modelpart);
// Write out the parent's index
XElement parentindex = new XElement("ParentIndex");
parentindex.Value = Convert.ToString(ParentNum.Value);
modelpart.Add(parentindex);
// Create the matrix
CMatrix4x4 matrix = new CMatrix4x4();
// Rotate
if (radiansToolStripMenuItem.Checked)
{
matrix.RotateLocalX((float)RotXNum.Value);
matrix.RotateLocalY((float)RotYNum.Value);
matrix.RotateLocalZ((float)RotZNum.Value);
}
else
{
matrix.RotateLocalX((float)Math.PI * (float)RotXNum.Value / 180.0f);
matrix.RotateLocalY((float)Math.PI * (float)RotYNum.Value / 180.0f);
matrix.RotateLocalZ((float)Math.PI * (float)RotZNum.Value / 180.0f);
}
// Translate
matrix.Translate((float)PosXNum.Value, (float)PosYNum.Value);
// Scale
matrix.Scale((float)ScaleXNum.Value, (float)ScaleYNum.Value);
// Write out the local transform
XElement localtrans = new XElement("LocalTransform");
modelpart.Add(localtrans);
// Write out the X Axis
XElement xaxis = new XElement("XAxis");
localtrans.Add(xaxis);
XElement xx = new XElement("x");
xx.Value = Convert.ToString(matrix.GetIndex(0, 0));
xaxis.Add(xx);
XElement xy = new XElement("y");
xy.Value = Convert.ToString(matrix.GetIndex(0, 1));
xaxis.Add(xy);
XElement xz = new XElement("z");
xz.Value = Convert.ToString(matrix.GetIndex(0, 2));
xaxis.Add(xz);
XElement xw = new XElement("w");
xw.Value = Convert.ToString(matrix.GetIndex(0, 3));
xaxis.Add(xw);
// Write out the Y Axis
XElement yaxis = new XElement("YAxis");
localtrans.Add(yaxis);
XElement yx = new XElement("x");
yx.Value = Convert.ToString(matrix.GetIndex(1, 0));
yaxis.Add(yx);
XElement yy = new XElement("y");
yy.Value = Convert.ToString(matrix.GetIndex(1, 1));
yaxis.Add(yy);
XElement yz = new XElement("z");
yz.Value = Convert.ToString(matrix.GetIndex(1, 2));
yaxis.Add(yz);
XElement yw = new XElement("w");
yw.Value = Convert.ToString(matrix.GetIndex(1, 3));
yaxis.Add(yw);
// Write out the Z Axis
XElement zaxis = new XElement("ZAxis");
localtrans.Add(zaxis);
XElement zx = new XElement("x");
zx.Value = Convert.ToString(matrix.GetIndex(2, 0));
zaxis.Add(zx);
XElement zy = new XElement("y");
zy.Value = Convert.ToString(matrix.GetIndex(2, 1));
zaxis.Add(zy);
XElement zz = new XElement("z");
zz.Value = Convert.ToString(matrix.GetIndex(2, 2));
zaxis.Add(zz);
XElement zw = new XElement("w");
zw.Value = Convert.ToString(matrix.GetIndex(2, 3));
zaxis.Add(zw);
// Write out the W Axis
XElement waxis = new XElement("WAxis");
localtrans.Add(waxis);
XElement wx = new XElement("x");
wx.Value = Convert.ToString(matrix.GetIndex(3, 0));
waxis.Add(wx);
XElement wy = new XElement("y");
wy.Value = Convert.ToString(matrix.GetIndex(3, 1));
waxis.Add(wy);
XElement wz = new XElement("z");
wz.Value = Convert.ToString(0.9);
waxis.Add(wz);
XElement ww = new XElement("w");
ww.Value = Convert.ToString(matrix.GetIndex(3, 3));
waxis.Add(ww);
// Write out the form file
XElement formfile = new XElement("GDFormFile");
formfile.Value = "GDForm/frm_" + _Path;
modelpart.Add(formfile);
// Save it out
root.Save(_Filename);
}
