public static Pose From(Pose3d pose)
{
return(From(pose.position, pose.rotation));
}
public static Pose WorldToLocal(this Pose pose, Pose3d relativeTo)
{
return(pose.Prepend(Inverse(relativeTo)));
}
public static Pose Prepend(this Pose pose, Pose3d prepend)
{
return(Append(prepend, pose));
}
public static Pose LocalToWorld(this Pose pose, Pose3d relativeTo)
{
return(pose.Prepend(relativeTo));
}
//public static Pose Append(this Pose pose, Pose3d append) { return From(pose).Append(append); }
public static Pose Append(this Pose pose, Pose3d append)
{
return(From(pose.position + pose.rotation * append.position, pose.rotation * append.rotation));
}