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); }