public SpatialVectorDouble componentMultiplication(SpatialVectorDouble other)
{
Debug.Assert(other.matrix.arr.Length == matrix.arr.Length);
SpatialVectorDouble result = new SpatialVectorDouble((uint)matrix.arr.Length);
for (int i = 0; i < matrix.arr.Length; i++)
{
result[i] = matrix.arr[i] * other.matrix.arr[i];
}
return(result);
}
public static double dot(SpatialVectorDouble a, SpatialVectorDouble b)
{
Trace.Assert(a.matrix.arr.Length == b.matrix.arr.Length);
double result = (double)0;
for (int i = 0; i < a.matrix.arr.Length; i++)
{
result += (a.matrix.arr[i] * b.matrix.arr[i]);
}
return(result);
}
public static Quaternion makeFromEulerAngles(double x, double y, double z)
{
// from http://web.archive.org/web/20060914224155/http://web.archive.org:80/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html
// Q60. How do I convert Euler rotation angles to a quaternion?
SpatialVectorDouble
basevectorX = new SpatialVectorDouble(new double[] { 1, 0, 0 }),
basevectorY = new SpatialVectorDouble(new double[] { 0, 1, 0 }),
basevectorZ = new SpatialVectorDouble(new double[] { 0, 0, 1 });
Quaternion
quaternionX = makeFromAxisAndAngle(basevectorX, x),
quaternionY = makeFromAxisAndAngle(basevectorY, y),
quaternionZ = makeFromAxisAndAngle(basevectorZ, z);
return((quaternionX * quaternionY) * quaternionZ);
}
public static Quaternion makeFromAxisAndAngle(SpatialVectorDouble axis, double angle)
{
// source http://web.archive.org/web/20060914224155/http://web.archive.org:80/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q50
// Q56. How do I convert a rotation axis and angle to a quaternion?
SpatialVectorDouble normalizedAxis = axis.normalized();
double sinAngle = System.Math.Sin(angle / 2.0);
double cosAngle = System.Math.Cos(angle / 2.0);
Quaternion result = new Quaternion();
result.i = normalizedAxis.x * sinAngle;
result.j = normalizedAxis.y * sinAngle;
result.k = normalizedAxis.z * sinAngle;
result.scalar = cosAngle;
return(result);
}
public static Matrix calcLocalToGlobalRotationAndTranslationMatrix(SpatialVectorDouble translation, SpatialVectorDouble rotation)
{
return(calcLocalToGlobalTranslationMatrix(translation) * calcLocalToGlobalRotationMatrix(rotation));
}
public static Matrix calcGlobalToLocalTranslationMatrix(SpatialVectorDouble translation)
{
return(calcLocalToGlobalTranslationMatrix(translation).inverse());
}
public static Matrix calcLocalToGlobalTranslationMatrix(SpatialVectorDouble translation)
{
return(Matrix44.createTranslation(translation.x, translation.y, translation.z));
}
public static Matrix calcGlobalToLocalRotationMatrix(SpatialVectorDouble rotation)
{
return(calcLocalToGlobalRotationMatrix(rotation).inverse());
}