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); }