public Bone(BinaryReader reader)
{
Name = reader.ReadString();
parentName = reader.ReadString();
InheritScale = reader.ReadBoolean();
InheritRotation = reader.ReadBoolean();
Position = reader.ReadVector3();
RotationMatrix = reader.ReadMatrix3();
Rotation = Quaternion.FromMatrix3(RotationMatrix);
}
public static void Conjugate(ref Quaternion quaternoin, out Quaternion result)
{
result.X = -quaternoin.X;
result.Y = -quaternoin.Y;
result.Z = -quaternoin.Z;
result.W = quaternoin.W;
}
public static void Transform(ref Vector3 vector, ref Quaternion quaternion, out Vector3 result)
{
float x2 = quaternion.X + quaternion.X;
float y2 = quaternion.Y + quaternion.Y;
float z2 = quaternion.Z + quaternion.Z;
float xx2 = quaternion.X * x2;
float xy2 = quaternion.X * y2;
float xz2 = quaternion.X * z2;
float yy2 = quaternion.Y * y2;
float yz2 = quaternion.Y * z2;
float zz2 = quaternion.Z * z2;
float wx2 = quaternion.W * x2;
float wy2 = quaternion.W * y2;
float wz2 = quaternion.W * z2;
result.X = vector.X * ((1f - yy2) - zz2) + vector.Y * (xy2 - wz2) + vector.Z * (xz2 + wy2);
result.Y = vector.X * (xy2 + wz2) + vector.Y * ((1f - xx2) - zz2) + vector.Z * (yz2 - wx2);
result.Z = vector.X * (xz2 - wy2) + vector.Y * (yz2 + wx2) + vector.Z * ((1f - xx2) - yy2);
}
public RigidTransform3(Vector3 position)
{
Position = position;
Orientation = Quaternion.Identity;
}
public Quaternion Multiply(Quaternion quaternion)
{
return new Quaternion
(
W*quaternion.X + X*quaternion.W + Y*quaternion.Z - Z*quaternion.Y,
W*quaternion.Y - X*quaternion.Z + Y*quaternion.W + Z*quaternion.X,
W*quaternion.Z + X*quaternion.Y - Y*quaternion.X + Z*quaternion.W,
W*quaternion.W - X*quaternion.X - Y*quaternion.Y - Z*quaternion.Z
);
}
public static void Sub(float value1, ref Quaternion value2, out Quaternion result)
{
result.X = value1 - value2.X;
result.Y = value1 - value2.Y;
result.Z = value1 - value2.Z;
result.W = value1 - value2.W;
}
// -
public static void Sub(ref Quaternion value1, ref Quaternion value2, out Quaternion result)
{
result.X = value1.X - value2.X;
result.Y = value1.Y - value2.Y;
result.Z = value1.Z - value2.Z;
result.W = value1.W - value2.W;
}
public static void Slerp(ref Quaternion start, ref Quaternion end, float interpolationAmount, out Quaternion result)
{
float cosHalfTheta = start.W * end.W + start.X * end.X + start.Y * end.Y + start.Z * end.Z;
if (cosHalfTheta < 0)
{
//Negating a quaternion results in the same orientation, but we need cosHalfTheta to be positive to get the shortest path.
end = -end;
cosHalfTheta = -cosHalfTheta;
}
// If the orientations are similar enough, then just pick one of the inputs.
if (cosHalfTheta > .999999f)
{
result = start;
return;
}
// Calculate temporary values.
float halfTheta = (float)Math.Acos(cosHalfTheta);
float sinHalfTheta = (float)Math.Sqrt(1.0 - cosHalfTheta * cosHalfTheta);
//Check to see if we're 180 degrees away from the target.
if (Math.Abs(sinHalfTheta) < 0.00001f)
{
result = (start + end) * .5f;
return;
}
//Blend the two quaternions to get the result!
float aFraction = (float)Math.Sin((1 - interpolationAmount) * halfTheta) / sinHalfTheta;
float bFraction = (float)Math.Sin(interpolationAmount * halfTheta) / sinHalfTheta;
result = start * aFraction + end * bFraction;
}
public static void FromRotationAxis(float axisX, float axisY, float axisZ, float angle, out Quaternion result)
{
angle *= .5f;
var sin = (float)Math.Sin(angle);
result.X = axisX * sin;
result.Y = axisY * sin;
result.Z = axisZ * sin;
result.W = (float)Math.Cos(angle);
}
public static void FromRotationAxis(ref Vector3 axis, float angle, out Quaternion result)
{
angle *= .5f;
var sin = (float)Math.Sin(angle);
result.X = axis.X * sin;
result.Y = axis.Y * sin;
result.Z = axis.Z * sin;
result.W = (float)Math.Cos(angle);
}
public static void FromMatrix4(ref Matrix4 matrix, out Quaternion result)
{
float w = (float)Math.Sqrt(1 + matrix.X.X + matrix.Y.Y + matrix.Z.Z) * .5f;
float delta = 1 / (w * 4);
result.X = (matrix.Z.Y - matrix.Y.Z) * delta;
result.Y = (matrix.X.Z - matrix.Z.X) * delta;
result.Z = (matrix.Y.X - matrix.X.Y) * delta;
result.W = w;
}
public static void FromEuler(float eulerX, float eulerY, float eulerZ, out Quaternion result)
{
eulerX *= .5f;
eulerY *= .5f;
eulerZ *= .5f;
float c1 = (float)Math.Cos(eulerY);
float s1 = (float)Math.Sin(eulerY);
float c2 = (float)Math.Cos(eulerZ);
float s2 = (float)Math.Sin(eulerZ);
float c3 = (float)Math.Cos(eulerX);
float s3 = (float)Math.Sin(eulerX);
float c1c2 = c1*c2;
float s1s2 = s1*s2;
float s1c2 = s1*c2;
float c1s2 = c1*s2;
result.W = c1c2*c3 - s1s2*s3;
result.X = c1c2*s3 + s1s2*c3;
result.Y = s1c2*c3 + c1s2*s3;
result.Z = c1s2*c3 - s1c2*s3;
}
public static void FromEuler(Vector3 euler, out Quaternion result)
{
euler.X *= .5f;
euler.Y *= .5f;
euler.Z *= .5f;
float c1 = (float)Math.Cos(euler.Y);
float s1 = (float)Math.Sin(euler.Y);
float c2 = (float)Math.Cos(euler.Z);
float s2 = (float)Math.Sin(euler.Z);
float c3 = (float)Math.Cos(euler.X);
float s3 = (float)Math.Sin(euler.X);
float c1c2 = c1*c2;
float s1s2 = s1*s2;
float s1c2 = s1*c2;
float c1s2 = c1*s2;
result.W = c1c2*c3 - s1s2*s3;
result.X = c1c2*s3 + s1s2*c3;
result.Y = s1c2*c3 + c1s2*s3;
result.Z = c1s2*c3 - s1c2*s3;
}
public static void Div(ref Quaternion value1, float value2, out Quaternion result)
{
result.X = value1.X / value2;
result.Y = value1.Y / value2;
result.Z = value1.Z / value2;
result.W = value1.W / value2;
}
// /
public static void Div(ref Quaternion value1, ref Quaternion value2, out Quaternion result)
{
result.X = value1.X / value2.X;
result.Y = value1.Y / value2.Y;
result.Z = value1.Z / value2.Z;
result.W = value1.W / value2.W;
}
public static void NormalizeSafe(ref Quaternion quaternion, out Quaternion result, out float length)
{
float dis = (float)Math.Sqrt((quaternion.X*quaternion.X) + (quaternion.Y*quaternion.Y) + (quaternion.Z*quaternion.Z) + (quaternion.W*quaternion.W));
length = dis;
if (dis == 0) result = new Quaternion();
else result = quaternion * (1/dis);
}
public static void RotationAxis(ref Quaternion quaternion, out Vector3 axis, out float angle)
{
angle = (float)Math.Acos(quaternion.W) * MathUtilities.Pi2;
float sinAngle = (float)Math.Sqrt(1 - (quaternion.W*quaternion.W));
if (sinAngle == 0) sinAngle = 1;
sinAngle = 1 / sinAngle;
axis = new Vector3(quaternion.X*sinAngle, quaternion.Y*sinAngle, quaternion.Z*sinAngle);
}
public static void FromSphericalRotation(float latitude, float longitude, float angle, out Quaternion result)
{
angle *= .5f;
float sa = (float)Math.Sin(angle);
float cLat = (float)Math.Cos(latitude);
float sLat = (float)Math.Sin(latitude);
float cLong = (float)Math.Cos(longitude);
float sLong = (float)Math.Sin(longitude);
result.X = sa*cLat*sLong;
result.Y = sa*sLat;
result.Z = sa*sLat*cLong;
result.W = (float)Math.Cos(angle);
}
public static void SphericalRotation(ref Quaternion quaternion, out float latitude, out float longitude, out float angle)
{
angle = (float)Math.Acos(quaternion.W) * MathUtilities.Pi2;
float sinAngle = (float)Math.Sqrt(1 - (quaternion.W*quaternion.W));
if (sinAngle == 0) sinAngle = 1;
sinAngle = 1 / sinAngle;
float x = quaternion.X * sinAngle;
float y = quaternion.Y * sinAngle;
float z = quaternion.Z * sinAngle;
latitude = -(float)Math.Asin(y);
if ((x*x) + (z*z) == 0) longitude = 0;
else longitude = (float)Math.Atan2(x, z) * MathUtilities.Pi;
if (longitude < 0) longitude += MathUtilities.Pi2;
}
public static void Length(ref Quaternion quaternion, out float result)
{
result = (float)Math.Sqrt((quaternion.X*quaternion.X) + (quaternion.Y*quaternion.Y) + (quaternion.Z*quaternion.Z) + (quaternion.W*quaternion.W));
}
public static void Sub(ref Quaternion value1, float value2, out Quaternion result)
{
result.X = value1.X - value2;
result.Y = value1.Y - value2;
result.Z = value1.Z - value2;
result.W = value1.W - value2;
}
// *
public static void Mul(ref Quaternion value1, ref Quaternion value2, out Quaternion result)
{
result.X = value1.X * value2.X;
result.Y = value1.Y * value2.Y;
result.Z = value1.Z * value2.Z;
result.W = value1.W * value2.W;
}
public Quaternion Concatenate(Quaternion quaternion)
{
return new Quaternion
(
quaternion.W*X + quaternion.X*W + quaternion.Y*Z - quaternion.Z*Y,
quaternion.W*Y - quaternion.X*Z + quaternion.Y*W + quaternion.Z*X,
quaternion.W*Z + quaternion.X*Y - quaternion.Y*X + quaternion.Z*W,
quaternion.W*W - quaternion.X*X - quaternion.Y*Y - quaternion.Z*Z
);
}
public static void Mul(ref Quaternion value1, float value2, out Quaternion result)
{
result.X = value1.X * value2;
result.Y = value1.Y * value2;
result.Z = value1.Z * value2;
result.W = value1.W * value2;
}
public RigidTransform3(Quaternion orienation, Vector3 position)
{
Orientation = orienation;
Position = position;
}
public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
{
result.X = quaternion1.W*quaternion2.X + quaternion1.X*quaternion2.W + quaternion1.Y*quaternion2.Z - quaternion1.Z*quaternion2.Y;
result.Y = quaternion1.W*quaternion2.Y - quaternion1.X*quaternion2.Z + quaternion1.Y*quaternion2.W + quaternion1.Z*quaternion2.X;
result.Z = quaternion1.W*quaternion2.Z + quaternion1.X*quaternion2.Y - quaternion1.Y*quaternion2.X + quaternion1.Z*quaternion2.W;
result.W = quaternion1.W*quaternion2.W - quaternion1.X*quaternion2.X - quaternion1.Y*quaternion2.Y - quaternion1.Z*quaternion2.Z;
}
public RigidTransform3(Quaternion orienation)
{
Position = new Vector3();
Orientation = orienation;
}
public static void Neg(ref Quaternion value, out Quaternion result)
{
result.X = -value.X;
result.Y = -value.Y;
result.Z = -value.Z;
result.W = -value.W;
}
public Vector3 Transform(Quaternion quaternion)
{
float x2 = quaternion.X + quaternion.X;
float y2 = quaternion.Y + quaternion.Y;
float z2 = quaternion.Z + quaternion.Z;
float xx2 = quaternion.X * x2;
float xy2 = quaternion.X * y2;
float xz2 = quaternion.X * z2;
float yy2 = quaternion.Y * y2;
float yz2 = quaternion.Y * z2;
float zz2 = quaternion.Z * z2;
float wx2 = quaternion.W * x2;
float wy2 = quaternion.W * y2;
float wz2 = quaternion.W * z2;
return new Vector3
(
X * ((1f - yy2) - zz2) + Y * (xy2 - wz2) + Z * (xz2 + wy2),
X * (xy2 + wz2) + Y * ((1f - xx2) - zz2) + Z * (yz2 - wx2),
X * (xz2 - wy2) + Y * (yz2 + wx2) + Z * ((1f - xx2) - yy2)
);
}
public static void Normalize(ref Quaternion quaternion, out Quaternion result)
{
result = quaternion * (1 / (float)Math.Sqrt((quaternion.X*quaternion.X) + (quaternion.Y*quaternion.Y) + (quaternion.Z*quaternion.Z) + (quaternion.W*quaternion.W)));
}
