public static Rotation3 SphericalLinear(double t, double t1, Rotation3 r1, double t2, Rotation3 r2)
{
var q1 = r1.Quaternion;
var q2 = r2.Quaternion;
var dot = Quaternion.Dot(q1, q2);
if (Quaternion.Dot(q1, q2) < 0)
{
q2 = -q2;
dot = -dot;
}
var dt = t2 - t1;
var angle = BasicMath.Acos(dot);
Quaternion q;
if (angle > BasicMath.Epsilon)
{
q = Math.Sin(angle * (t2 - t) / dt) * q1 + Math.Sin(angle * (t - t1) / dt) * q2;
}
else
{
q = ((t2 - t) * q1 + (t - t1) * q2) / dt;
}
return((Rotation3)q);
}
public StochasticManifoldPoint Apply(IManifoldPoint point, double time)
{
var expectation = new Rotation3(time * _angularVelocity).Apply((Vector3)(OVector)point);
var covariance = SymmetricMatrix.Scalar(3, time * _diffusionCoefficient);
return(new StochasticManifoldPoint((OVector)expectation, covariance));
}
private int MakeDynamic4StarAlignment(Vect3 star0, string name0, PairA stand0, double eqAngle0,
Vect3 star1, string name1, PairA stand1, double eqAngle1,
Vect3 star2, string name2, PairA stand2, double eqAngle2,
Vect3 star3, string name3, PairA stand3, double eqAngle3,
out Alignment calc)
{
Vect3 eqNorth = new Vect3(0, latitude_);
Vect3 eqAxis = eqNorth;
double equAngleFactor = 1;
calc = null;
int i = 0;
for (; i < 50; ++i)
{
Vect3 star1_corrected = new Rotation3((eqAngle1 - eqAngle0) * equAngleFactor, eqAxis).Apply(star1);
Vect3 star3_corrected = new Rotation3((eqAngle3 - eqAngle2) * equAngleFactor, eqAxis).Apply(star3);
calc = new DSCAlignment(new Vect3(0, latitude_), precesions_);
calc.AddStar(new AlignStar(name0, star0, stand0, eqAngle0));
calc.AddStar(new AlignStar(name1, star1_corrected, stand1, eqAngle0));
calc.AddStar(new AlignStar(name2, star2, stand2, eqAngle2));
calc.AddStar(new AlignStar(name3, star3_corrected, stand3, eqAngle2));
Vect3 equAxis = calc.EquAxis;
PairA correction = new PairA(-equAxis.Azm, latitude_ - equAxis.Alt);
Vect3 eqAxisNew = new Vect3(eqNorth.Azm - correction.Azm, eqNorth.Alt - correction.Alt);
double diff = Vect3.VMul(eqAxis, eqAxisNew).Abs;
if (diff < toRad / 240)
{
break;
}
eqAxis = eqAxisNew;
equAngleFactor = calc.EquAngleFactor;
}
return(i);
}
private PairA ToScope(PairA h, double equAngle, Vect3 equAxis, Rotation3 standRotation)
{
return((new Rotation3(-equAngle, equAxis) * standRotation).Conj.Apply(h).Offset(azmOff_, altOff_));;
}
private PairA FromScope(PairA s, double equAngle, Vect3 equAxis, Rotation3 standRotation)
{
return((new Rotation3(-equAngle, equAxis) * standRotation).Apply(s.Offset(-azmOff_, -altOff_)));
}
public static Vector3 RightAngularVelocity(double t1, Rotation3 r1, double t2, Rotation3 r2)
{
return((r1.Inv * r2).Vector / (t2 - t1));
}
public static Vector3 LeftAngularVelocity(double t1, Rotation3 r1, double t2, Rotation3 r2)
{
return((r2 * r1.Inv).Vector / (t2 - t1));
}
public RotoTranslation3(Rotation3 rotation) : this(rotation, Vector3.Zero)
{
}
public RotoTranslation3(Rotation3 rotation, Vector3 translation)
{
_rotation = rotation;
_translation = translation;
}
