public static Vector3R RotateReverse(Vector3R vector, QuaternionR quat, bool assumeUnitQuat)
{
Vector3 v = new Vector3(vector.X, vector.Y, vector.Z);
Quaternion q = new Quaternion(quat.X, quat.Y, quat.Z, assumeUnitQuat ? 1 : quat.W);
Vector3 result = Vector3.Transform(v, Quaternion.Conjugate(q));
return(new Vector3R {
X = result.X, Y = result.Y, Z = result.Z
});
}
public static Vector3R TransformToWorldFrame(Vector3R vBody, Pose pose, bool assumeUnitQuat = true)
{
Vector3R translated = new Vector3R
{
X = vBody.X + pose.Position.X,
Y = vBody.Y + pose.Position.Y,
Z = vBody.Z + pose.Position.Z,
};
return(TransformToWorldFrame(translated, pose.Orientation, assumeUnitQuat));
}
public static float Magnitude(Vector3R vector)
{
Vector3 v = new Vector3(vector.X, vector.Y, vector.Z);
return(v.Length());
}
public static Vector3R TransformToWorldFrame(Vector3R vBody, QuaternionR quat, bool assumeUnitQuat = true)
{
return(Rotate(vBody, quat, assumeUnitQuat));
}
public static Vector3R TransformToBodyFrame(Vector3R vWorld, QuaternionR quat, bool assumeUnitQuat = true)
{
return(RotateReverse(vWorld, quat, assumeUnitQuat));
}
