public bool Equals(QuaternionF other)
{
if (ReferenceEquals(null, other)) return false;
if (ReferenceEquals(this, other)) return true;
return Equals(other.Vector, Vector) && Math.Abs(other.Scalar - Scalar) < 0.0001;
}
public CoordinateF Rotate(CoordinateF coord)
{
// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
var q = new QuaternionF(coord.Normalise(), 0);
var temp = q * Conjugate();
return (this * temp).Vector;
}
public static QuaternionF Slerp(QuaternionF start, QuaternionF end, float blend)
{
// Clone to avoid modifying the parameters
var q1 = start.Clone();
var q2 = end.Clone();
// if either input is zero, return the other.
if (Math.Abs(q1.Magnitude() - 0) < 0.0001) return Math.Abs(q2.Magnitude() - 0) < 0.0001 ? Identity : q2;
if (Math.Abs(q2.Magnitude() - 0) < 0.0001) return q1;
var cosHalfAngle = q1.Dot(q2);
if (cosHalfAngle >= 1 || cosHalfAngle <= -1) return q1;
if (cosHalfAngle < 0)
{
q2.Vector = -q2.Vector;
q2.Scalar = -q2.Scalar;
cosHalfAngle = -cosHalfAngle;
}
float blendA;
float blendB;
if (cosHalfAngle < 0.99)
{
// do proper slerp for big angles
var halfAngle = (float) Math.Acos(cosHalfAngle);
var sinHalfAngle = (float)Math.Sin(halfAngle);
var oneOverSinHalfAngle = 1 / sinHalfAngle;
blendA = (float) Math.Sin(halfAngle * (1 - blend)) * oneOverSinHalfAngle;
blendB = (float) Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1 - blend;
blendB = blend;
}
var result = new QuaternionF(blendA * q1.Vector + blendB * q2.Vector, blendA * q1.W + blendB * q2.W);
return result.Magnitude() > 0 ? result.Normalise() : Identity;
}
public float Dot(QuaternionF c)
{
return Vector.Dot(c.Vector) + Scalar * c.Scalar;
}
public static MatrixF Rotation(QuaternionF quaternion)
{
var aa = quaternion.GetAxisAngle();
return(Rotation(aa.Item1, aa.Item2));
}
public static QuaternionF Lerp(QuaternionF start, QuaternionF end, float blend)
{
// Clone to avoid modifying the parameters
var q1 = start.Clone();
var q2 = end.Clone();
// if either input is zero, return the other.
if (Math.Abs(q1.Magnitude() - 0) < 0.0001) return Math.Abs(q2.Magnitude() - 0) < 0.0001 ? Identity : q2;
if (Math.Abs(q2.Magnitude() - 0) < 0.0001) return q1;
var blendA = 1 - blend;
var blendB = blend;
var result = new QuaternionF(blendA * q1.Vector + blendB * q2.Vector, blendA * q1.W + blendB * q2.W);
return result.Magnitude() > 0 ? result.Normalise() : Identity;
}
private static void ReadBone(BinaryReader br, int index, ModelData data, DataStructures.Models.Model model)
{
var name = "";
var nameIndex = 0;
if (data.Version >= MDLVersionSource2006)
{
nameIndex = br.ReadInt32();
}
else if (data.Version == MDLVersionGoldsource)
{
name = br.ReadFixedLengthString(Encoding.UTF8, 32);
}
var parent = br.ReadInt32();
int flags = 0;
if (data.Version == MDLVersionGoldsource)
{
flags = br.ReadInt32();
}
var boneController = br.ReadIntArray(6); // 3 pos, 3 rot
var defPos = br.ReadCoordinateF();
QuaternionF quat = null;
if (data.Version >= MDLVersionSource2006)
{
// quaternion
quat = new QuaternionF(br.ReadCoordinateF(), br.ReadSingle());
}
var defAng = br.ReadCoordinateF();
var defPosScale = br.ReadCoordinateF();
var defAngScale = br.ReadCoordinateF();
if (data.Version >= MDLVersionSource2006)
{
var poseToBone = br.ReadIntArray(12); // 3x4 matrix
var qAlignment = new QuaternionF(br.ReadCoordinateF(), br.ReadSingle());
flags = br.ReadInt32();
var proctype = br.ReadInt32();
var procindex = br.ReadInt32();
var physicsbone = br.ReadInt32();
var surfacepropidx = br.ReadInt32();
var contents = br.ReadInt32();
br.ReadIntArray(8); // Unused
}
var parentBone = parent < 0 ? null : model.Bones[parent];
model.Bones.Add(new Bone(index, parent, parentBone, name, defPos, defAng, defPosScale, defAngScale));
}
public void ReadData(BinaryReader br)
{
var delta = (Flags & StudioAnimDelta) > 0;
if ((Flags & StudioAnimRawrot) > 0)
{
// WTF is this messy format :|
// 48-bit Quaternion: 16-bit x, 16-bit y, 15-bit z, 1 bit to flag if w is negative
// Convert into real quaternion with the algorithm below (taken from compressed_vector.h)
int x = br.ReadUInt16();
int y = br.ReadUInt16();
var temp = br.ReadUInt16();
var z = temp & 0x7FFF; // Get the last 15 bits from the short
var isWneg = (temp & 0x8000) > 0; // The first bit is the boolean value
var w = (isWneg ? -1 : 1) * (float) Math.Sqrt(1 - x * x - y * y - z * z);
FixedQuaternion = new QuaternionF((x - Half) / Half, (y - Half) / Half, (z - Quarter) / Quarter, w);
}
if ((Flags & StudioAnimRawpos) > 0)
{
// What's this? A custom made, 16-bit floating point implementation? WHAT DID I DO TO DESERVE THIS???
var bytes = br.ReadBytes(6);
// Wait a minute.....
var x = OpenTK.Half.FromBytes(bytes, 0).ToSingle();
var y = OpenTK.Half.FromBytes(bytes, 2).ToSingle();
var z = OpenTK.Half.FromBytes(bytes, 4).ToSingle();
// Ha ha, screw you, custom floating-point implementation! Thanks, OpenTK!
FixedPosition = new CoordinateF(x, y, z);
}
if ((Flags & StudioAnimAnimrot) > 0)
{
// Why is this so painful :(
// Read the per-frame data using RLE, just like GoldSource models
var startPos = br.BaseStream.Position;
var offsets = br.ReadShortArray(3);
var endPos = br.BaseStream.Position;
var rotFrames = new List<float[]>();
for (var i = 0; i < NumFrames; i++) rotFrames.Add(new float[] {0, 0, 0});
for (var i = 0; i < 3; i++)
{
if (offsets[i] == 0) continue;
br.BaseStream.Position = startPos + offsets[i];
var values = ReadRLEEncodedAnimationFrameValues(br, NumFrames);
for (var f = 0; f < values.Count; f++)
{
rotFrames[f][i] =+ values[f];
if (f > 0 && delta) rotFrames[f][i] += values[f - 1];
}
}
FrameAngles.AddRange(rotFrames.Select(x => new CoordinateF(x[0], x[1], x[2])));
br.BaseStream.Position = endPos;
}
if ((Flags & StudioAnimAnimpos) > 0)
{
// Same as above, except for the position coordinate
var startPos = br.BaseStream.Position;
var offsets = br.ReadShortArray(3);
var endPos = br.BaseStream.Position;
var posFrames = new List<float[]>();
for (var i = 0; i < NumFrames; i++) posFrames.Add(new float[] { 0, 0, 0 });
for (var i = 0; i < 3; i++)
{
if (offsets[i] == 0) continue;
br.BaseStream.Position = startPos + offsets[i];
var values = ReadRLEEncodedAnimationFrameValues(br, NumFrames);
for (var f = 0; f < values.Count; f++)
{
posFrames[f][i] = +values[f];
if (f > 0 && delta) posFrames[f][i] += values[f - 1];
}
}
FramePositions.AddRange(posFrames.Select(x => new CoordinateF(x[0], x[1], x[2])));
br.BaseStream.Position = endPos;
}
}
public BoneAnimationFrame(Bone bone, CoordinateF position, QuaternionF angles)
{
Bone = bone;
Position = position;
Angles = angles;
}
public static MatrixF Rotation(QuaternionF quaternion)
{
var aa = quaternion.GetAxisAngle();
return Rotation(aa.Item1, aa.Item2);
}
public float Dot(QuaternionF c)
{
return(Vector.Dot(c.Vector) + Scalar * c.Scalar);
}
