public void CopyFrom(Quaternion4F q)
{
W = q.W;
X = q.X;
Y = q.Y;
Z = q.Z;
}
public void FromQuaternion(Quaternion4F q)
{
SetIdentity();
// Compute a few values to optimize common subexpressions
var ww = 2.0f * q.W;
var xx = 2.0f * q.X;
var yy = 2.0f * q.Y;
var zz = 2.0f * q.Z;
// Set the matrix elements. There is still a little more
// opportunity for optimization due to the many common
// subexpressions. We'll let the compiler handle that...
Values[0] = 1.0f - (yy * q.Y) - (zz * q.Z);
Values[1] = (xx * q.Y) + (ww * q.Z);
Values[2] = (xx * q.Z) - (ww * q.Y);
Values[4] = (xx * q.Y) - (ww * q.Z);
Values[5] = 1.0f - (xx * q.X) - (zz * q.Z);
Values[6] = (yy * q.Z) + (ww * q.X);
Values[8] = (xx * q.Z) + (ww * q.Y);
Values[9] = (yy * q.Z) - (ww * q.X);
Values[10] = 1.0f - (xx * q.X) - (yy * q.Y);
}
/// <summary>
/// Do Spherical linear interpolation between two quaternions
/// </summary>
/// <param name="q1">The first quaternion</param>
/// <param name="q2">The second quaternion</param>
/// <param name="blend">The blend factor</param>
/// <returns>A smooth blend between the given quaternions</returns>
public static Quaternion4F OtkSlerp(Quaternion4F q1, Quaternion4F q2, float blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == 0.0f)
{
if (q2.LengthSquared == 0.0f)
{
return(Identity);
}
return(q2);
}
if (q2.LengthSquared == 0.0f)
{
return(q1);
}
var cosHalfAngle = q1.W * q2.W + q1.Xyz.Dot(q2.Xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(q1);
}
if (cosHalfAngle < 0.0f)
{
q2.Xyz.Negate();
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
float blendA;
float blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
var halfAngle = (float)Math.Acos(cosHalfAngle);
var sinHalfAngle = (float)Math.Sin(halfAngle);
var oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
blendB = (float)Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - blend;
blendB = blend;
}
var result = new Quaternion4F(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > 0.0f)
{
result.Normalize();
return(result);
}
return(Identity);
}
/// <summary>
/// Devuelve un nuevo Quaternion que representa la concatenación de rotaciones de ambos.
/// </summary>
/// <param name="tempLeft"></param>
/// <param name="tempRight"></param>
/// <returns></returns>
public static Quaternion4F operator *(Quaternion4F left, Quaternion4F right)
{
var q = new Quaternion4F();
q.X = left.W * right.X + left.X * right.W + left.Y * right.Z - left.Z * right.Y;
q.Y = left.W * right.Y + left.Y * right.W + left.Z * right.X - left.X * right.Z;
q.Z = left.W * right.Z + left.Z * right.W + left.X * right.Y - left.Y * right.X;
q.W = left.W * right.W - left.X * right.X - left.Y * right.Y - left.Z * right.Z;
return(q);
}
/// <summary>
/// this = this * q
/// </summary>
/// <param name="q"></param>
public void Multiply(Quaternion4F q)
{
var auxX = _w * q._x + _x * q._w + _y * q._z - _z * q._y;
var auxY = _w * q._y + _y * q._w + _z * q._x - _x * q._z;
var auxZ = _w * q._z + _z * q._w + _x * q._y - _y * q._x;
var auxW = _w * q._w - _x * q._x - _y * q._y - _z * q._z;
_x = auxX;
_y = auxY;
_z = auxZ;
_w = auxW;
}
public static Quaternion4F FromRadiansAxis(float radianes, float x, float y, float z)
{
var quaternion = new Quaternion4F();
var num2 = radianes * 0.5f;
var num = (float)Math.Sin(num2);
var num3 = (float)Math.Cos(num2);
quaternion._x = x * num;
quaternion._y = y * num;
quaternion._z = z * num;
quaternion._w = num3;
quaternion.Normalize();
return(quaternion);
}
/// <summary>
/// Multiplica esta matriz (this) por la representada por el quaternion pasado como parámetro.
/// <para>this = this * Quaternion</para>
/// </summary>
/// <param name="qRot"></param>
public virtual void Rotate(Quaternion4F qRot)
{
//qRot.GetMatriz4(ref auxMat4);
AuxMat4.FromQuaternion(qRot);
MultipliesBy(AuxMat4);
}
public static Quaternion4F Slerp(Quaternion4F q0, Quaternion4F q1, float t)
{
var qresult = new Quaternion4F();
// Check for out-of range parameter and return edge points if so
if (t <= 0.0)
{
qresult.CopyFrom(q0);
return(qresult);
}
if (t >= 1.0)
{
qresult.CopyFrom(q1);
return(qresult);
}
// Compute "cosine of angle between quaternions" using dot product
var cosOmega = Dot(q0, q1);
// If negative dot, use -q1. Two quaternions q and -q
// represent the same rotation, but may produce
// different slerp. We chose q or -q to rotate using
// the acute angle.
var q1W = q1._w;
var q1X = q1._x;
var q1Y = q1._y;
var q1Z = q1._z;
if (cosOmega < 0.0)
{
q1W = -q1W;
q1X = -q1X;
q1Y = -q1Y;
q1Z = -q1Z;
cosOmega = -cosOmega;
}
// We should have two unit quaternions, so dot should be <= 1.0
if (!(cosOmega < 1.1f))
{
throw new Exception("Error en Quaternion.Slerp(), cosOmega > 1.1f");
}
// Compute interpolation fraction, checking for quaternions
// almost exactly the same
float k0, k1;
if (cosOmega > 0.9999)
{
// Very close - just use linear interpolation,
// which will protect againt a divide by zero
k0 = 1.0f - t;
k1 = t;
}
else
{
// Compute the sin of the angle using the
// trig identity sin^2(omega) + cos^2(omega) = 1
var sinOmega = (float)Math.Sqrt(1.0 - (cosOmega * cosOmega));
// Compute the angle from its sin and cosine
var omega = (float)Math.Atan2(sinOmega, cosOmega);
// Compute inverse of denominator, so we only have
// to divide once
var oneOverSinOmega = 1.0f / sinOmega;
// Compute interpolation parameters
k0 = (float)(Math.Sin((1.0 - t) * omega) * oneOverSinOmega);
k1 = (float)(Math.Sin(t * omega) * oneOverSinOmega);
}
// Interpolate and return new quaternion
return(new Quaternion4F(
(k0 * q0._w) + (k1 * q1W),
(k0 * q0._x) + (k1 * q1X),
(k0 * q0._y) + (k1 * q1Y),
(k0 * q0._z) + (k1 * q1Z)
));
}
public static float Dot(Quaternion4F a, Quaternion4F b)
{
return((a._w * b._w) + (a._x * b._x) + (a._y * b._y) + (a._z * b._z));
}
public float Dot(Quaternion4F quat)
{
return(_w * quat._w + _x * quat._x + _y * quat._y + _z * quat._z);
}