/// <inheritdoc />
public void Calculate(IReadOnlyList <IBody> bodies)
{
var p = bodies[_handle];
_handle.Delta = _position - p.Position;
_handle.AngleDelta = Quaterniond.CreateFromTo(p.Rotation, _rotation).ToAxisAngle();
}
/// <inheritdoc/>
/// <summary>
///
/// </summary>
/// <param name="bodies"></param>
public void Calculate(IReadOnlyList <IBody> bodies)
{
var b0 = bodies[_h0];
var b1 = bodies[_h1];
var p0 = b0.Position;
var r0 = b0.Rotation;
var p1 = b1.Position;
var r1 = b1.Rotation;
// apply position deltas
var dp = p1 - p0;
_h0.Delta = (r1.Apply(_p01) + dp) * 0.5;
_h1.Delta = (r0.Apply(_p10) - dp) * 0.5;
// apply rotation deltas
var dr0 = Quaterniond.CreateFromTo(r0, r1.Apply(_r01));
dr0.ToAxisAngle(out Vec3d a0, out double t0);
_h0.AngleDelta = a0 * (t0 * 0.5);
var dr1 = Quaterniond.CreateFromTo(r1, r0.Apply(_r10));
dr1.ToAxisAngle(out Vec3d a1, out double t1);
_h1.AngleDelta = a1 * (t1 * 0.5);
}
/// <inheritdoc />
public void Calculate(ReadOnlyArrayView <Body> bodies)
{
var r0 = bodies[_i0].Rotation.Current;
var r1 = bodies[_i1].Rotation.Current;
// apply rotation deltas
_dr0 = Quaterniond.CreateFromTo(r0, r1.Apply(_tr0)).ToAxisAngle() * 0.5;
}
/// <inheritdoc />
public void Calculate(IReadOnlyList <IBody> bodies)
{
var r0 = bodies[_h0].Rotation;
var r1 = bodies[_h1].Rotation;
// apply rotation deltas
var dr = Quaterniond.CreateFromTo(r1, r0.Apply(_r10)).ToAxisAngle() * 0.5;
_h0.AngleDelta = -dr;
_h1.AngleDelta = dr;
}
/// <inheritdoc />
public void Calculate(IReadOnlyList <IBody> bodies)
{
var b0 = bodies[_h0];
var b1 = bodies[_h1];
var r0 = b0.Rotation;
var r1 = b1.Rotation;
var dp = b1.Position - b0.Position;
_h0.Delta = (r1.Apply(_d01) + dp) * 0.5;
_h1.Delta = (r0.Apply(_d10) - dp) * 0.5;
var dr = Quaterniond.CreateFromTo(r1, r0.Apply(_r10)).ToAxisAngle() * 0.5;
_h0.AngleDelta = -dr;
_h1.AngleDelta = dr;
}
/// <inheritdoc />
public void Calculate(ReadOnlyArrayView <Body> bodies)
{
_delta = Quaterniond.CreateFromTo(bodies[_index].Rotation.Current, _target).ToAxisAngle();
}
/// <inheritdoc />
public void Calculate(IReadOnlyList <IBody> bodies)
{
_handle.AngleDelta = Quaterniond.CreateFromTo(bodies[_handle].Rotation, _rotation).ToAxisAngle();
}
