public static v8 <Force> operator *(Txfm6x8 <T> lhs, v8 <Force> rhs)
{
// Spatial transform for force vectors:
//' a2b = [E 0] * [1 -rx] = [E -E*rx]
//' [0 E] [0 1 ] [0 E ]
//' (E = rotation matrix 3x3)
// So:
// a2b * v = [E -E*rx] * [v.ang] = [E*v.ang - E*rx*v.lin]
// [0 E ] [v.lin] [E*v.lin ]
return(new v8 <Force>(
lhs.a2b.rot * rhs.ang - lhs.a2b.rot * Math_.Cross(lhs.a2b.pos, rhs.lin),
lhs.a2b.rot * rhs.lin));
}
// Operators
public static v8 <Motion> operator *(Txfm6x8 <T> lhs, v8 <Motion> rhs)
{
// Spatial transform for motion vectors:
//' a2b = [E 0] * [ 1 0] = [ E 0]
//' [0 E] [-rx 1] [-E*rx E]
//' (E = rotation matrix 3x3)
// So:
// a2b * v = [ E 0] * [v.ang] = [E*v.ang ]
// [-E*rx E] [v.lin] [E*v.lin - E*rx*v.ang]
return(new v8 <Motion>(
lhs.a2b.rot * rhs.ang,
lhs.a2b.rot * rhs.lin - lhs.a2b.rot * Math_.Cross(lhs.a2b.pos, rhs.ang)));
}
// Orientation matrix to "look" at a point
public static m4x4 LookAt(v4 eye, v4 at, v4 up)
{
Debug.Assert(eye.w == 1.0f && at.w == 1.0f && up.w == 0.0f, "Invalid position/direction vectors passed to LookAt");
Debug.Assert(eye - at != v4.Zero, "LookAt 'eye' and 'at' positions are coincident");
Debug.Assert(!Math_.Parallel(eye - at, up), "LookAt 'forward' and 'up' axes are aligned");
var mat = new m4x4 {
};
mat.z = Math_.Normalise(eye - at);
mat.x = Math_.Normalise(Math_.Cross(up, mat.z));
mat.y = Math_.Cross(mat.z, mat.x);
mat.pos = eye;
return(mat);
}
/// <summary>Construct a quaternion representing a rotation from 'from' to 'to'</summary>
public quat(v4 from, v4 to)
: this()
{
var d = Math_.Dot(from.xyz, to.xyz);
var s = (float)Math.Sqrt(from.xyz.LengthSq * to.xyz.LengthSq) + d;
var axis = Math_.Cross(from, to);
// vectors are 180 degrees apart
if (Math_.FEql(s, 0))
{
axis = Math_.Perpendicular(to);
s = 0.0f;
}
xyzw = Math_.Normalise(new v4(axis.x, axis.y, axis.z, s));
}
/// <summary>Create a transform representing the rotation from one vector to another. 'from' and 'to' do not have to be normalised</summary>
public static m3x4 Rotation(v4 from, v4 to)
{
Debug.Assert(!Math_.FEql(from.Length, 0));
Debug.Assert(!Math_.FEql(to.Length, 0));
var len = from.Length * to.Length;
// Find the cosine of the angle between the vectors
var cos_angle = Math_.Dot(from, to) / len;
if (cos_angle >= 1f - Math_.TinyF)
{
return(Identity);
}
if (cos_angle <= Math_.TinyF - 1f)
{
return(Rotation(Math_.Normalise(Math_.Perpendicular(from - to)), (float)Math_.TauBy2));
}
// Axis multiplied by sine of the angle
var axis_sine_angle = Math_.Cross(from, to) / len;
var axis_norm = Math_.Normalise(axis_sine_angle);
return(Rotation(axis_norm, axis_sine_angle, cos_angle));
}
