Skip to content

Instantly share code, notes, and snippets.

@hYdos
Created January 10, 2023 07:10
Show Gist options
  • Save hYdos/b4d2a370e35255d51a023bd96d476242 to your computer and use it in GitHub Desktop.
Save hYdos/b4d2a370e35255d51a023bd96d476242 to your computer and use it in GitHub Desktop.
public STKeyFrame GetFVGFL(STAnimationTrack t, float frame, float startFrame = 0)
{
if (t.KeyFrames.Count == 0) return new STKeyFrame(frame, 0);
if (t.KeyFrames.Count == 1) return t.KeyFrames[0];
STKeyFrame LK = t.KeyFrames.First();
float Frame = frame - startFrame;
foreach (STKeyFrame keyFrame in t.KeyFrames)
{
if (keyFrame.Frame <= Frame) LK = keyFrame;
}
return LK;
}
public STKeyFrame GetFVGFR(STAnimationTrack t, float frame, float startFrame = 0)
{
if (t.KeyFrames.Count == 0) return new STKeyFrame(frame, 0);
if (t.KeyFrames.Count == 1) return t.KeyFrames[0];
STKeyFrame RK = t.KeyFrames.Last();
float Frame = frame - startFrame;
foreach (STKeyFrame keyFrame in t.KeyFrames)
{
if (keyFrame.Frame >= Frame && keyFrame.Frame < RK.Frame) RK = keyFrame;
}
return RK;
}
// private static readonly ushort _flagsMask = 0b11000011_11111111;
private static short UnpackS15(short u15)
{
int sign = (u15 >> 14) & 1;
u15 &= 0x3FFF;
if (sign == 0) u15 -= 0x4000;
return u15;
}
static int[][] QUATERNION_SWIZZLES = {
new int[] { 0, 3, 2, 1 }, new int[] { 3, 0, 2, 1 },
new int[] { 3, 2, 0, 1 }, new int[] { 3, 2, 1, 0 }
};
private static Quaternion PackedToQuat(short z, short y, short x)
{
const int count = 15;
const int BASE = (1 << count) - 1;
float maxval = 1 / (0x399E * (float)Math.Sqrt(2.0)); // such obvious, so constant, wow
long cq = x & 0xFFFF;
cq <<= 16;
cq |= ((uint)y) & 0xFFFF;
cq <<= 16;
cq |= ((uint)z) & 0xFFFF;
short extra = (short)(cq & 0x7);
long num = cq >> 3;
x = UnpackS15((short)((num >> (count * 2)) & BASE));
y = UnpackS15((short)((num >> (count * 1)) & BASE));
z = UnpackS15((short)((num >> (count * 0)) & BASE));
float fx = x * maxval;
float fy = y * maxval;
float fz = z * maxval;
float[] quat = {
(float)Math.Sqrt(1 - fx * fx - fy * fy - fz * fz),
fx,
fy,
fz };
int[] qmap = QUATERNION_SWIZZLES[(extra & 3)];
Quaternion q = new Quaternion(quat[qmap[0]], quat[qmap[1]], quat[qmap[2]], quat[qmap[3]]);
if ((extra >> 2) != 0) q *= -1;
return q;
}
public static Quaternion EulerToQuat(float z, float y, float x)
{
Quaternion xRotation = Quaternion.FromAxisAngle(Vector3.UnitX, x);
Quaternion yRotation = Quaternion.FromAxisAngle(Vector3.UnitY, y);
Quaternion zRotation = Quaternion.FromAxisAngle(Vector3.UnitZ, z);
return (zRotation * yRotation * xRotation);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment