void UpdateOrientation(targ_type dt)
{
DVector3 axis = Omega.normalized;
// Omega is in radians/sec, but AngleAxis need degrees.
targ_type w = (targ_type)(Omega.magnitude * (360 / (2 * Math.PI)));
// Note that in Unity, a positive angle of rotation is clockwise around the
// axis of rotation. This is unusual....
var inc = DQuaternion.AngleAxis(w * dt, axis);
PrevOrientation = Orientation;
Orientation = inc * Orientation;
}
// Perform a single iteration: Find the energy delta, measure the gradient WRT
// each perturbation. Find the one with the largest effect and apply a scaled adjustment
// to the orientation.
//
DQuaternion Iterate(DQuaternion cur_orientation)
{
double ecur = EnergyFromOrientation(cur_orientation);
double del = Energy - ecur;
double dm = 0;
double delbest = 0;
int ibest = 0;
for (int im = 0; im < Adjustments.Length; im++)
{
var o = Adjustments[im] * cur_orientation;
var ep = EnergyFromOrientation(o);
var cd = ep - ecur;
if (Math.Abs(cd) > dm)
{
ibest = im;
delbest = cd;
dm = Math.Abs(cd);
}
}
double factor = del / delbest;
if (factor < -4)
{
factor = -4; // There is no clamp for doubles.
}
else if (factor > 4)
{
factor = 4;
}
double angle = AdjustAngle * factor;
var adjust = DQuaternion.AngleAxis(angle, AdjustmentDirs[ibest]);
var next_orientation = adjust * cur_orientation;
return(next_orientation);
}