public static MatrixD BSpline(MatrixD a, MatrixD b, float t)
{
var up = Vector3D.Lerp(a.Up, b.Up, t);
var bez = new CubicCurve(a, b);
var x = bez.Sample(t);
var fwd = bez.SampleDerivative(t);
return(MatrixD.CreateWorld(x, Vector3D.Normalize(fwd), Vector3D.Normalize(up)));
}
public CubicSphericalCurve(Vector3D center, MatrixD from, MatrixD to)
{
var tmp = new CubicCurve(from, to);
var avgSpherical = Vector3D.Normalize((from.Translation + to.Translation) / 2 - center);
var quat = QuaternionD.CreateFromTwoVectors(avgSpherical, new Vector3D(1, 0, 0));
var m = MatrixD.CreateFromQuaternion(quat);
m = MatrixD.CreateTranslation(-center) * m;
_matrix = MatrixD.Invert(m);
_curve = new CubicCurve(SphericalExtensions.ToSpherical(Vector3D.Transform(tmp.P0, m)),
SphericalExtensions.ToSpherical(Vector3D.Transform(tmp.P1, m)),
SphericalExtensions.ToSpherical(Vector3D.Transform(tmp.P2, m)),
SphericalExtensions.ToSpherical(Vector3D.Transform(tmp.P3, m)));
}
