/**
* Returns a new Matrix composed by the passed scale (vector3), rotation (quaternion) and translation (vector3).
*/
public static BabylonMatrix Compose(BabylonVector3 scale, BabylonQuaternion rotation, BabylonVector3 translation)
{
var result = BabylonMatrix.Identity();
BabylonMatrix.ComposeToRef(scale, rotation, translation, result);
return(result);
}
public static BabylonQuaternion FromEulerAngles(float x, float y, float z)
{
var q = new BabylonQuaternion();
BabylonQuaternion.RotationYawPitchRollToRef(y, x, z, q);
return(q);
}
public static BabylonQuaternion Slerp(BabylonQuaternion left, BabylonQuaternion right, float amount)
{
float num2;
float num3;
float num4 = (((left.X * right.X) + (left.Y * right.Y)) + (left.Z * right.Z)) + (left.W * right.W);
bool flag = false;
if (num4 < 0)
{
flag = true;
num4 = -num4;
}
if (num4 > 0.999999)
{
num3 = 1 - amount;
num2 = flag ? -amount : amount;
}
else
{
var num5 = Math.Acos((double)num4);
var num6 = (1.0 / Math.Sin((double)num5));
num3 = (float)((Math.Sin((1.0 - (double)amount) * (double)num5)) * (double)num6);
num2 = (float)(flag ? ((-Math.Sin((double)amount * num5)) * num6) : ((Math.Sin((double)amount * num5)) * num6));
}
BabylonQuaternion result = new BabylonQuaternion()
{
X = (num3 * left.X) + (num2 * right.X),
Y = (num3 * left.Y) + (num2 * right.Y),
Z = (num3 * left.Z) + (num2 * right.Z),
W = (num3 * left.W) + (num2 * right.W)
};
return(result);
}
// https://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors?s_tid=mwa_osa_a
// https://github.com/mrdoob/three.js/blob/09cfc67a3f52aeb4dd0009921d82396fd5dc5172/src/math/Quaternion.js#L199-L272
public BabylonQuaternion toQuaternion(EulerRotationOrder rotationOrder = EulerRotationOrder.XYZ)
{
BabylonQuaternion quaternion = new BabylonQuaternion();
var c1 = Math.Cos(0.5 * this.X);
var c2 = Math.Cos(0.5 * this.Y);
var c3 = Math.Cos(0.5 * this.Z);
var s1 = Math.Sin(0.5 * this.X);
var s2 = Math.Sin(0.5 * this.Y);
var s3 = Math.Sin(0.5 * this.Z);
switch (rotationOrder)
{
case EulerRotationOrder.XYZ:
quaternion.X = (float)(s1 * c2 * c3 + c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 - s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 + s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 - s1 * s2 * s3);
break;
case EulerRotationOrder.YZX:
quaternion.X = (float)(s1 * c2 * c3 + c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 + s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 - s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 - s1 * s2 * s3);
break;
case EulerRotationOrder.ZXY:
quaternion.X = (float)(s1 * c2 * c3 - c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 + s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 + s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 - s1 * s2 * s3);
break;
case EulerRotationOrder.XZY:
quaternion.X = (float)(s1 * c2 * c3 - c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 - s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 + s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 + s1 * s2 * s3);
break;
case EulerRotationOrder.YXZ:
quaternion.X = (float)(s1 * c2 * c3 + c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 - s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 - s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 + s1 * s2 * s3);
break;
case EulerRotationOrder.ZYX:
quaternion.X = (float)(s1 * c2 * c3 - c1 * s2 * s3);
quaternion.Y = (float)(c1 * s2 * c3 + s1 * c2 * s3);
quaternion.Z = (float)(c1 * c2 * s3 - s1 * s2 * c3);
quaternion.W = (float)(c1 * c2 * c3 + s1 * s2 * s3);
break;
}
return(quaternion);
}
public BabylonQuaternion toQuaternionGltf()
{
BabylonQuaternion babylonQuaternion = RotationYawPitchRollToRefBabylon(X, -Y, -Z);
// Doing following computation is ugly but works
// The goal is to switch from left to right handed coordinate system
// Swap X and Y
var tmp = babylonQuaternion.X;
babylonQuaternion.X = babylonQuaternion.Y;
babylonQuaternion.Y = tmp;
return(babylonQuaternion);
}
public BabylonQuaternion MultiplyWith(BabylonQuaternion quaternion)
{
BabylonQuaternion result = new BabylonQuaternion();
// (a + i b + j c + k d)*(e + i f + j g + k h) = a*e - b*f - c*g- d*h + i (b*e + a*f + c*h - d*g) + j (a*g - b*h + c*e + d*f) + k (a*h + b*g - c*f + d*e)
// W*q.W - X*q.X - Y*q.Y- Z*q.Z + i (X*q.W + W*q.X + Y*q.Z - Z*q.Y) + j (W*q.Y - X*q.Z + Y*q.W + Z*q.X) + k (W*q.Z + X*q.Y - Y*q.X + Z*q.W)
result.W = W * quaternion.W - X * quaternion.X - Y * quaternion.Y - Z * quaternion.Z;
result.X = X * quaternion.W + W * quaternion.X + Y * quaternion.Z - Z * quaternion.Y;
result.Y = W * quaternion.Y - X * quaternion.Z + Y * quaternion.W + Z * quaternion.X;
result.Z = W * quaternion.Z + X * quaternion.Y - Y * quaternion.X + Z * quaternion.W;
return(result);
}
/**
* Update a Matrix with values composed by the passed scale (vector3), rotation (quaternion) and translation (vector3).
*/
public static void ComposeToRef(BabylonVector3 scale, BabylonQuaternion rotation, BabylonVector3 translation, BabylonMatrix result)
{
var matrix1 = new BabylonMatrix();
BabylonMatrix.FromValuesToRef(scale.X, 0, 0, 0,
0, scale.Y, 0, 0,
0, 0, scale.Z, 0,
0, 0, 0, 1, matrix1);
var matrix0 = new BabylonMatrix();
rotation.toRotationMatrix(matrix0);
matrix1.multiplyToRef(matrix0, result);
result.setTranslation(translation);
}
/**
* Updates the passed quaternion "result" with the passed rotation matrix values.
*/
public static void FromRotationMatrixToRef(BabylonMatrix matrix, BabylonQuaternion result)
{
var data = matrix.m;
float m11 = data[0], m12 = data[4], m13 = data[8];
float m21 = data[1], m22 = data[5], m23 = data[9];
float m31 = data[2], m32 = data[6], m33 = data[10];
var trace = m11 + m22 + m33;
float s;
if (trace > 0)
{
s = (float)(0.5 / Math.Sqrt(trace + 1.0));
result.W = 0.25f / s;
result.X = (m32 - m23) * s;
result.Y = (m13 - m31) * s;
result.Z = (m21 - m12) * s;
}
else if (m11 > m22 && m11 > m33)
{
s = (float)(2.0 * Math.Sqrt(1.0 + m11 - m22 - m33));
result.W = (m32 - m23) / s;
result.X = 0.25f * s;
result.Y = (m12 + m21) / s;
result.Z = (m13 + m31) / s;
}
else if (m22 > m33)
{
s = (float)(2.0 * Math.Sqrt(1.0 + m22 - m11 - m33));
result.W = (m13 - m31) / s;
result.X = (m12 + m21) / s;
result.Y = 0.25f * s;
result.Z = (m23 + m32) / s;
}
else
{
s = (float)(2.0 * Math.Sqrt(1.0 + m33 - m11 - m22));
result.W = (m21 - m12) / s;
result.X = (m13 + m31) / s;
result.Y = (m23 + m32) / s;
result.Z = 0.25f * s;
}
}
/**
* Returns a new Quaternion object, computed from the Vector3 coordinates.
*/
public BabylonQuaternion toQuaternion()
{
var result = new BabylonQuaternion();
var cosxPlusz = Math.Cos((this.X + this.Z) * 0.5);
var sinxPlusz = Math.Sin((this.X + this.Z) * 0.5);
var coszMinusx = Math.Cos((this.Z - this.X) * 0.5);
var sinzMinusx = Math.Sin((this.Z - this.X) * 0.5);
var cosy = Math.Cos(this.Y * 0.5);
var siny = Math.Sin(this.Y * 0.5);
result.X = (float)(coszMinusx * siny);
result.Y = (float)(-sinzMinusx * siny);
result.Z = (float)(sinxPlusz * cosy);
result.W = (float)(cosxPlusz * cosy);
return(result);
}
/**
* (Copy pasted from babylon)
* Sets the passed quaternion "result" from the passed float Euler angles (y, x, z).
*/
private BabylonQuaternion RotationYawPitchRollToRefBabylon(float yaw, float pitch, float roll)
{
// Produces a quaternion from Euler angles in the z-y-x orientation (Tait-Bryan angles)
var halfRoll = roll * 0.5;
var halfPitch = pitch * 0.5;
var halfYaw = yaw * 0.5;
var sinRoll = Math.Sin(halfRoll);
var cosRoll = Math.Cos(halfRoll);
var sinPitch = Math.Sin(halfPitch);
var cosPitch = Math.Cos(halfPitch);
var sinYaw = Math.Sin(halfYaw);
var cosYaw = Math.Cos(halfYaw);
var result = new BabylonQuaternion();
result.X = (float)((cosYaw * sinPitch * cosRoll) + (sinYaw * cosPitch * sinRoll));
result.Y = (float)((sinYaw * cosPitch * cosRoll) - (cosYaw * sinPitch * sinRoll));
result.Z = (float)((cosYaw * cosPitch * sinRoll) - (sinYaw * sinPitch * cosRoll));
result.W = (float)((cosYaw * cosPitch * cosRoll) + (sinYaw * sinPitch * sinRoll));
return(result);
}
/**
* Decomposes the current Matrix into :
* - a scale vector3 passed as a reference to update,
* - a rotation quaternion passed as a reference to update,
* - a translation vector3 passed as a reference to update.
* Returns the boolean `true`.
*/
public bool decompose(BabylonVector3 scale, BabylonQuaternion rotation, BabylonVector3 translation)
{
translation.X = this.m[12];
translation.Y = this.m[13];
translation.Z = this.m[14];
scale.X = (float)Math.Sqrt(this.m[0] * this.m[0] + this.m[1] * this.m[1] + this.m[2] * this.m[2]);
scale.Y = (float)Math.Sqrt(this.m[4] * this.m[4] + this.m[5] * this.m[5] + this.m[6] * this.m[6]);
scale.Z = (float)Math.Sqrt(this.m[8] * this.m[8] + this.m[9] * this.m[9] + this.m[10] * this.m[10]);
if (this.determinant() <= 0)
{
scale.Y *= -1;
}
if (scale.X == 0 || scale.Y == 0 || scale.Z == 0)
{
rotation.X = 0;
rotation.Y = 0;
rotation.Z = 0;
rotation.W = 1;
return(false);
}
var matrix = new BabylonMatrix();
BabylonMatrix.FromValuesToRef(
this.m[0] / scale.X, this.m[1] / scale.X, this.m[2] / scale.X, 0,
this.m[4] / scale.Y, this.m[5] / scale.Y, this.m[6] / scale.Y, 0,
this.m[8] / scale.Z, this.m[9] / scale.Z, this.m[10] / scale.Z, 0,
0, 0, 0, 1, matrix);
BabylonQuaternion.FromRotationMatrixToRef(matrix, rotation);
return(true);
}
public static float[] Interpolate(BabylonAnimation animation, BabylonAnimationKey fromKey, BabylonAnimationKey toKey, float frame)
{
switch (animation.property)
{
case "_matrix":
var fromMatrix = new BabylonMatrix();
fromMatrix.m = new List <float>(fromKey.values).ToArray();
var toMatrix = new BabylonMatrix();
toMatrix.m = new List <float>(toKey.values).ToArray();
var fromPosition = new BabylonVector3();
var fromRotation = new BabylonQuaternion();
var fromScaling = new BabylonVector3();
var toPosition = new BabylonVector3();
var toRotation = new BabylonQuaternion();
var toScaling = new BabylonVector3();
fromMatrix.decompose(fromScaling, fromRotation, fromPosition);
toMatrix.decompose(toScaling, toRotation, toPosition);
var lerpFactor = MathUtilities.GetLerpFactor(fromKey.frame, toKey.frame, frame);
var interpolatedKeyPosition = BabylonVector3.FromArray(MathUtilities.Lerp(fromPosition.ToArray(), toPosition.ToArray(), lerpFactor));
var interpolatedKeyRotation = BabylonQuaternion.Slerp(fromRotation, toRotation, lerpFactor);
var interpolatedKeyScaling = BabylonVector3.FromArray(MathUtilities.Lerp(fromScaling.ToArray(), toScaling.ToArray(), lerpFactor));
return(BabylonMatrix.Compose(interpolatedKeyScaling, interpolatedKeyRotation, interpolatedKeyPosition).m);
case "rotationQuaternion":
return(BabylonQuaternion.Slerp(BabylonQuaternion.FromArray(fromKey.values), BabylonQuaternion.FromArray(toKey.values), MathUtilities.GetLerpFactor(fromKey.frame, toKey.frame, frame)).ToArray());
case "scaling":
case "position":
default:
return(MathUtilities.Lerp(fromKey.values, toKey.values, MathUtilities.GetLerpFactor(fromKey.frame, toKey.frame, frame)));
}
}
