/// <summary>
/// The operation that performs after arrows setting
/// </summary>
public virtual void PostLoadPosition()
{
CreateFrame();
CopyPositionToRelativeFrame();
CopyQuaternionToRelativeFrame();
Init();
relative.SetMatrix();
}
/* private object sd()
* {
* return result[9, 1];
* }
*
* private object md()
* {
* return result[10, 1];
* }
*
* private object nd()
* {
* return result[11, 1];
* }
*
* private object od()
* {
* return result[12, 1];
* }
*
* private object ld()
* {
* return result[13, 1];
* }*/
#endregion
private void SetRelative()
{
ReferenceFrame relative = Relative;
Array.Copy(relativePosition, relative.Position, 3);
IVelocity v = relative as IVelocity;
// Array.Copy(relativeVelocity, v.RevativeVelocity, 3);
Array.Copy(relativeQuaternion, relative.Quaternion, 3);
IOrientation or = relative as IOrientation;
Array.Copy(relativeQuaternion, or.Quaternion, 4);
relative.SetMatrix();
}
/// <summary>
/// Updates itself
/// </summary>
public override void Update()
{
try
{
this.FullReset();
IDataConsumer cons = this;
cons.UpdateChildrenData();
ReferenceFrame relative = Relative;
IPosition p = relative;
double[] x = p.Position;
ReferenceFrame parent = this.GetParentFrame();
for (int i = 0; i < 3; i++)
{
x[i] = (double)measurements[i].Parameter();
}
if (relative is IVelocity)
{
IVelocity vel = relative as IVelocity;
double[] v = vel.Velocity;
for (int i = 0; i < 3; i++)
{
IDerivation d = measurements[i] as IDerivation;
v[i] = Measurement.GetDouble(d.Derivation);
}
}
double[] qua = relative.Quaternion;
for (int i = 0; i < qua.Length; i++)
{
IMeasurement m = measurements[i + 3];
qua[i] = (double)m.Parameter();
}
relative.SetMatrix();
if (relative is IAngularVelocity)
{
IAngularVelocity av = relative as IAngularVelocity;
double[] om = av.Omega;
IOrientation or = relative as IOrientation;
for (int i = 0; i < 4; i++)
{
IDerivation d = measurements[i + 3] as IDerivation;
der[i] = Measurement.GetDouble(d.Derivation);
}
Vector3D.StaticExtensionVector3D.CalculateDynamics(or.Quaternion, der, om, qd);
}
if (relative is IAcceleration)
{
IAcceleration acc = relative as IAcceleration;
IAngularAcceleration anc = relative as IAngularAcceleration;
double[] linacc = acc.RelativeAcceleration;
for (int i = 0; i < linacc.Length; i++)
{
linacc[i] = Measurement.GetDouble(secondDeriM[i]);
}
for (int i = 0; i < 4; i++)
{
angsec[i] = Measurement.GetDouble(secondDeriM[i + 3]);
}
IAngularVelocity av = relative as IAngularVelocity;
double[] om = av.Omega;
IOrientation or = relative as IOrientation;
double[] angacc = anc.AngularAcceleration;
Vector3D.StaticExtensionVector3D.CalculateAcceleratedDynamics(or.Quaternion, der, om, qd, angsec, angacc);
}
base.Update();
}
catch (Exception exception)
{
exception.ShowError(10);
//!!! OLD this.Throw(ex);
}
}
