public static PosRot Interpolate(PosRot px0y0, PosRot px1y0, PosRot px0y1, PosRot px1y1, float x, float y)
{
PosRot py0 = Interpolate(px0y0, px1y0, x);
PosRot py1 = Interpolate(px0y1, px1y1, x);
return(Interpolate(py0, py1, y));
}
// ----- ----- ----- ----- -----
public static PosRot Interpolate(PosRot px0, PosRot px1, float x)
{
return(new PosRot(
px1.position * x + px0.position * (1 - x),
Quaternion.Slerp(px0.rotation, px1.rotation, x)
));
}
public PosRot Inverse()
{
PosRot result = new PosRot();
result.rotation = Quaternion.Inverse(rotation);
result.position = -(result.rotation * position);
return(result);
}
public PosRot TransformPosRot(PosRot posrot)
{
PosRot result = new PosRot();
result.position = position + rotation * posrot.position;
result.rotation = rotation * posrot.rotation;
return(result);
}
public static PosRot Rotate(PosRot px0, Quaternion rotation, Vector3 center)
{
PosRot px1 = new PosRot();
px1.position = rotation * (px0.position - center) + center;
px1.rotation = rotation * px0.rotation;
return(px1);
}
public static PosRot Parse(string line)
{
var data = line.Split(',').Select(s => float.Parse(s));
PosRot pose = new PosRot();
pose.position = new Vector3(data.ElementAt(0), data.ElementAt(1), data.ElementAt(2));
pose.rotation = new Quaternion(data.ElementAt(3), data.ElementAt(4), data.ElementAt(5), data.ElementAt(6));
return(pose);
}
public PosRot(PosRot p)
{
position = p.position; rotation = p.rotation;
}