public CollisionGroupNode(EndianBinaryReader reader)
{
Children = new ObservableCollection <CollisionGroupNode>();
Triangles = new List <CollisionTriangle>();
int name_offset = reader.ReadInt32();
long cur_offset = reader.BaseStream.Position;
reader.BaseStream.Seek(name_offset, System.IO.SeekOrigin.Begin);
Name = Encoding.ASCII.GetString(reader.ReadBytesUntil(0));
reader.BaseStream.Seek(cur_offset, System.IO.SeekOrigin.Begin);
m_Scale = new Vector3(reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle());
Vector3 eulerRot = new Vector3();
for (int e = 0; e < 3; e++)
{
eulerRot[e] = WMath.RotationShortToFloat(reader.ReadInt16());
}
// ZYX order
m_Rotation = Quaterniond.FromAxisAngle(new Vector3d(0, 0, 1), WMath.DegreesToRadians(eulerRot.Z)) *
Quaterniond.FromAxisAngle(new Vector3d(0, 1, 0), WMath.DegreesToRadians(eulerRot.Y)) *
Quaterniond.FromAxisAngle(new Vector3d(1, 0, 0), WMath.DegreesToRadians(eulerRot.X));
m_Unknown1 = reader.ReadUInt16();
m_Translation = new Vector3(reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle());
m_ParentIndex = reader.ReadInt16();
m_NextSiblingIndex = reader.ReadInt16();
m_FirstChildIndex = reader.ReadInt16();
m_RoomNum = reader.ReadInt16();
FirstVertexIndex = reader.ReadInt16();
reader.SkipInt16(); // Octree index, we don't need it for loading from dzb
m_Bitfield = reader.ReadInt32();
}
/// <summary>
/// Returns the rotation component of this instance. Quite slow.
/// </summary>
/// <param name="row_normalise">Whether the method should row-normalise (i.e. remove scale from) the Matrix. Pass false if you know it's already normalised.</param>
public Quaterniond ExtractRotation(bool row_normalise = true)
{
var row0 = Row0;
var row1 = Row1;
var row2 = Row2;
if (row_normalise)
{
row0 = row0.Normalized();
row1 = row1.Normalized();
row2 = row2.Normalized();
}
// code below adapted from Blender
Quaterniond q = new Quaterniond();
double trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0);
if (trace > 0)
{
double sq = Math.Sqrt(trace);
q.W = sq;
sq = 1.0 / (4.0 * sq);
q.X = (row1[2] - row2[1]) * sq;
q.Y = (row2[0] - row0[2]) * sq;
q.Z = (row0[1] - row1[0]) * sq;
}
else if (row0[0] > row1[1] && row0[0] > row2[2])
{
double sq = 2.0 * Math.Sqrt(1.0 + row0[0] - row1[1] - row2[2]);
q.X = 0.25 * sq;
sq = 1.0 / sq;
q.W = (row2[1] - row1[2]) * sq;
q.Y = (row1[0] + row0[1]) * sq;
q.Z = (row2[0] + row0[2]) * sq;
}
else if (row1[1] > row2[2])
{
double sq = 2.0 * Math.Sqrt(1.0 + row1[1] - row0[0] - row2[2]);
q.Y = 0.25 * sq;
sq = 1.0 / sq;
q.W = (row2[0] - row0[2]) * sq;
q.X = (row1[0] + row0[1]) * sq;
q.Z = (row2[1] + row1[2]) * sq;
}
else
{
double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]);
q.Z = 0.25 * sq;
sq = 1.0 / sq;
q.W = (row1[0] - row0[1]) * sq;
q.X = (row2[0] + row0[2]) * sq;
q.Y = (row2[1] + row1[2]) * sq;
}
q.Normalize();
return(q);
}
