/// <summary>
/// Transforms a vector by a quaternion rotation.
/// </summary>
/// <param name="vec">The vector to transform.</param>
/// <param name="quat">The quaternion to rotate the vector by.</param>
/// <param name="result">The result of the operation.</param>
public static void QuaternionTransform(ref Float3 vec, ref Float4 quat, out Float3 result)
{
// Since vec.W == 0, we can optimize quat * vec * quat^-1 as follows:
// vec + 2.0 * cross(quat.xyz, cross(quat.xyz, vec) + quat.w * vec)
Float3 xyz = quat.Xyz, temp, temp2;
Float3.Cross(ref xyz, ref vec, out temp);
Float3.Multiply(ref vec, quat.W, out temp2);
Float3.Add(ref temp, ref temp2, out temp);
Float3.Cross(ref xyz, ref temp, out temp);
Float3.Multiply(ref temp, 2, out temp);
Float3.Add(ref vec, ref temp, out result);
}
/// <summary>
/// Convert the current quaternion to axis angle representation
/// </summary>
/// <param name="axis">The resultant axis</param>
/// <param name="angle">The resultant angle</param>
public static void ToAxisAngle(Float4 quaternion, out Float3 axis, out float angle)
{
var q = quaternion;
if (Math.Abs(q.W) > 1.0f)
q.Normalize();
angle = 2.0f * (float)System.Math.Acos(q.W); // angle
float den = (float)System.Math.Sqrt(1.0 - q.W * q.W);
if (den > 0.0001f)
{
axis = MathHelper.Scale(q.Xyz, 1.0f / den);
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
axis = Float3.UnitX;
}
}
/// <summary>
/// Transforms a vector by a quaternion rotation.
/// </summary>
/// <param name="vec">The vector to transform.</param>
/// <param name="quat">The quaternion to rotate the vector by.</param>
public static Float3 QuaternionTransform(Float3 vec, Float4 quat)
{
Float3 res;
QuaternionTransform(ref vec, ref quat, out res);
return res;
}
public static Float4x4 Rotate(Float4 quaternion)
{
Float3 axis;
float angle;
Float4.ToAxisAngle(quaternion, out axis, out angle);
return CreateFromAxisAngle(axis, angle);
}
/// <summary>
/// Multiplies an instance by a scalar.
/// </summary>
/// <param name="quaternion">The instance.</param>
/// <param name="scale">The scalar.</param>
/// <returns>A new instance containing the result of the calculation.</returns>
public static Float4 QuaternionMultiply(Float4 quaternion, float scale)
{
return new Float4(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale);
}
/// <summary>
/// Multiplies an instance by a scalar.
/// </summary>
/// <param name="quaternion">The instance.</param>
/// <param name="scale">The scalar.</param>
/// <param name="result">A new instance containing the result of the calculation.</param>
public static void QuaternionMultiply(ref Float4 quaternion, float scale, out Float4 result)
{
result = new Float4(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale);
}
/// <summary>
/// Multiplies two instances.
/// </summary>
/// <param name="left">The first instance.</param>
/// <param name="right">The second instance.</param>
/// <param name="result">A new instance containing the result of the calculation.</param>
public static void QuaternionMultiply(ref Float4 left, ref Float4 right, out Float4 result)
{
result = new Float4(
left.Xyz * right.W + right.Xyz * left.W + Float3.Cross(left.Xyz, right.Xyz),
left.W * right.W - Float3.Dot(left.Xyz, right.Xyz));
}
/// <summary>
/// Multiplies two instances.
/// </summary>
/// <param name="left">The first instance.</param>
/// <param name="right">The second instance.</param>
/// <returns>A new instance containing the result of the calculation.</returns>
public static Float4 QuaternionMultiply(Float4 left, Float4 right)
{
Float4 result;
Multiply(ref left, ref right, out result);
return result;
}
private IMeshStream CreateMeshStream(ISource source, string semantic)
{
var floatArray = source as FloatArraySource;
if (floatArray != null)
{
bool swapY = (semantic == Streams.TexCoord);
if (source.GetStride() == 3)
{
var arrayMeshStream = new ArrayMeshStream<Float3>(source.GetCount(), streamConverterFactory);
for (int i = 0; i < arrayMeshStream.Count; ++i)
{
var y = floatArray[i * 3 + 1];
if (swapY) y = 1.0f - y;
arrayMeshStream[i] = new Float3(floatArray[i * 3 + 0], y, floatArray[i * 3 + 2]);
}
return arrayMeshStream;
}
else if (source.GetStride() == 2)
{
var arrayMeshStream = new ArrayMeshStream<Float2>(source.GetCount(), streamConverterFactory);
for (int i = 0; i < arrayMeshStream.Count; ++i)
{
var y = floatArray[i * 2 + 1];
if (swapY) y = 1.0f - y;
arrayMeshStream[i] = new Float2(floatArray[i * 2 + 0], y);
}
return arrayMeshStream;
}
else if (source.GetStride() == 4)
{
var arrayMeshStream = new ArrayMeshStream<Float4>(source.GetCount(), streamConverterFactory);
for (int i = 0; i < arrayMeshStream.Count; ++i)
{
var y = floatArray[i * 4 + 1];
if (swapY) y = 1.0f - y;
arrayMeshStream[i] = new Float4(floatArray[i * 4 + 0], y, floatArray[i * 4 + 2], floatArray[i * 4 + 3]);
}
return arrayMeshStream;
}
}
else
{
throw new NotImplementedException();
}
throw new NotImplementedException();
}
