using System;
using System.Collections.Generic;
using System.Text;
using System.Runtime.Serialization;
using CategoryTheory;
using DiagramUI;
using DiagramUI.Aliases;
using DataPerformer;
using DataPerformer.Interfaces;
using RealMatrixProcessor;
using Vector3D;
using Motion6D.Intrefaces;
namespace Motion6D
{
/// <summary>
/// Reference frame of inertial object
/// The object have nonzero moment of inertia
/// </summary>
[Serializable()]
public class InertialReferenceFrame : RigidReferenceFrame, IDifferentialEquationSolver,
IDataConsumer, IStarted, IVelocity, IOrientation, IAngularVelocity
{
#region Fields
/// <summary>
/// Acceleration string
/// </summary>
string[] forcesString = new string[12];
/// <summary>
/// Moment of inertia string
/// </summary>
string[] momentOfInertia = new string[7];
/// <summary>
/// State string
/// </summary>
string[] state = new string[19];
/// <summary>
/// Results of calculation
/// </summary>
//private double[,] result = new double[13, 2];
/// <summary>
/// Initial conditions
/// </summary>
private double[] initialConditions = new double[] { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 };
/// <summary>
/// Internal measurements
/// </summary>
private IMeasure[] internalMeasurements = new IMeasure[13];
/// <summary>
/// External measures
/// </summary>
private IMeasure[] measures = new IMeasure[12];
/// <summary>
/// Measure of inretia
/// </summary>
private IMeasure[] inertia = new IMeasure[7];
/// <summary>
/// Measurements
/// </summary>
private List<IMeasurements> measurements = new List<IMeasurements>();
/// <summary>
/// The "is updated" sign
/// </summary>
private bool isUpdated = false;
/// <summary>
/// External aliases
/// </summary>
private AliasName[] aln = new AliasName[19];
/// <summary>
/// Auxiliary massive for linear and angular velocities
/// </summary>
//private double[] lin = new double[3];
/// <summary>
/// Linear acceleration
/// </summary>
private double[] linAccAbsolute = new double[3];
/// <summary>
/// Linear accelration
/// </summary>
private double[] linAccRelative = new double[3];
/// <summary>
/// Angular acceleration
/// </summary>
private double[] epsRelative = new double[3];
/// <summary>
/// Angular acceration
/// </summary>
private double[] epsAbsolute = new double[3];
private double[] aux4d = new double[4];
private double[] quaternionDervation = new double[4];
/// <summary>
/// Auxiliary massive for derivations of linear and angular velocities
/// </summary>
// private double[] accd = new double[3];
/// <summary>
/// Auxiliary massive for intermediate calculations
/// </summary>
private double[] aux = new double[3];
private double[] aux1 = new double[3];
private double[] aux2 = new double[3];
/// <summary>
/// Auxiliary massive for intermediate calculations
/// </summary>
//private double[] auxa = new double[3];
/// <summary>
/// Auxiliary massive for intermediate calculations
/// </summary>
// private double[] auxad = new double[3];
/// <summary>
/// Auxiliary massive for intermediate calculations
/// </summary>
// private double[] add = new double[3];
/// <summary>
/// Auxiliary massive for quaternions
/// </summary>
// private double[] qat = new double[4];
/// <summary>
/// Auxiliary massive for derivations of quaternions
/// </summary>
private double[] qdr = new double[4];
/// <summary>
/// Auxiliary massive for intermediate calculations
/// </summary>
private double[] qad = new double[4];
/// <summary>
/// The -1st power of mass of the object
/// </summary>
private double unMass = 1;
/// <summary>
/// Mass of the object (unlike the "unMass" field this field is made only for serialization)
/// </summary>
private double mass = 1;
/// <summary>
/// The tensor of inertia
/// </summary>
private double[,] J = new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
/// <summary>
/// Inverted matrix of inertia tensor (it is useful, because less calculations are involved)
/// </summary>
private double[,] L = new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
/// <summary>
/// Transition matrix
/// </summary>
//private double[,] T = new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
/// <summary>
/// Auxiliary matrix (it seems that this matrix could be useful, too)
/// </summary>
private double[,] am = new double[4, 4];
/// <summary>
/// Forces and moments, acting on the body
/// </summary>
private double[] forces = new double[12];
/// <summary>
/// Velocity
/// </summary>
private double[] velocity = new double[3];
/// <summary>
/// Relative velocity
/// </summary>
private double[] relativeVelocity = new double[3];
/// <summary>
/// Quaternion
/// </summary>
private double[] quater = new double[] { 1, 0, 0, 0 };
/// <summary>
/// Transition matrix
/// </summary>
private double[,] transitionMatrix = new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
/// <summary>
/// Angular velocity
/// </summary>
private double[] omega = new double[] { 0, 0, 0 };
#endregion
#region Ctor
/// <summary>
/// Default constructor
/// </summary>
public InertialReferenceFrame()
{
init();
}
/// <summary>
/// Deserialization constructor
/// </summary>
/// <param name="info">Serialization Info</param>
/// <param name="context">Streaming Context</param>
protected InertialReferenceFrame(SerializationInfo info, StreamingContext context)
: this()
{
forcesString = info.GetValue("Acceleration", typeof(string[])) as string[];
momentOfInertia = info.GetValue("MomentOfInertia", typeof(string[])) as string[];
state = info.GetValue("State", typeof(string[])) as string[];
initialConditions = info.GetValue("InitialConditions", typeof(double[])) as double[];
}
#endregion
#region Overriden Members
/// <summary>
/// Implementation of serialization
/// </summary>
/// <param name="info">Serialization Info</param>
/// <param name="context">Streaming Context</param>
public override void GetObjectData(SerializationInfo info, StreamingContext context)
{
info.AddValue("Acceleration", forcesString, typeof(string[]));
info.AddValue("MomentOfInertia", momentOfInertia, typeof(string[]));
info.AddValue("State", state, typeof(string[]));
info.AddValue("InitialConditions", initialConditions, typeof(double[]));
}
/*public override double[,] RelativeMatrix
{
get
{
return base.RelativeMatrix;
}
set
{
base.RelativeMatrix = value;
/ for (int i = 0; i < 4; i++)
{
initialConditions[i + 6] = relativeQuaternion[i];
}
}
}*/
#endregion
#region IDifferentialEquationSolver Members
void IDifferentialEquationSolver.CalculateDerivations()
{
//IReferenceFrame f = this;
//ReferenceFrame frame = f.Own;
SetAliases();
IDataConsumer cons = this;
int i = 0;
cons.Reset();
cons.UpdateChildrenData();
//Filling of massive of forces and moments using results of calculations of formula trees
for (i = 0; i < 12; i++)
{
forces[i] = (double)measures[i].Parameter();
}
//Filling the part, responding to derivation of radius-vector
/*for (i = 0; i < 3; i++)
{
result[i, 1] = result[3 + i, 0];
}*/
int k = 0;
for (i = 0; i < 3; i++)
{
for (int j = i; j < 3; j++)
{
IMeasure min = inertia[k];
++k;
if (min != null)
{
double jin = (double)min.Parameter();
J[i, j] = jin;
J[j, i] = jin;
}
}
}
IMeasure mm = inertia[6];
if (mm != null)
{
unMass = (double)mm.Parameter();
unMass = 1 / unMass;
}
//Filling the part, responding to derivation of linear velocity
for (i = 0; i < 3; i++)
{
linAccAbsolute[i] = forces[6 + i] * unMass;
}
double[,] T = Relative.Matrix;
RealMatrix.Multiply(linAccAbsolute, T, aux);
for (i = 0; i < 3; i++)
{
linAccAbsolute[i] = forces[i] * unMass + aux[i];
}
RealMatrix.Multiply(J, omega, aux);
V3DOperations.VectorPoduct(omega, aux, aux1);
RealMatrix.Add(aux1, 0, forces, 9, aux, 0, 3);
Array.Copy(forces, 3, aux1, 0, 3);
RealMatrix.Multiply(aux1, T, aux2);
RealMatrix.Add(aux2, 0, forces, 9, aux1, 0, 3);
RealMatrix.Add(aux1, 0, aux, 0, aux2, 0, 3);
RealMatrix.Multiply(L, aux2, epsRelative);
aux4d[0] = 0;
Array.Copy(omega, 0, aux4d, 1, 3);
V3DOperations.QuaternionInvertMultiply(relativeQuaternion, aux4d, quaternionDervation);
for (i = 0; i < 3; i++)
{
quaternionDervation[i] *= 0.5;
}
SetRelative();
Update();
}
void IDifferentialEquationSolver.CopyVariablesToSolver(int offset, double[] variables)
{
Array.Copy(variables, offset, relativePosition, 0, 3);
Array.Copy(variables, offset + 3, relativeVelocity, 0, 3);
RealMatrix.Normalize(variables, offset + 6, 4);
Array.Copy(variables, offset + 6, relativeQuaternion, 0, 4);
Array.Copy(variables, offset + 10, omega, 0, 3);
SetRelative();
}
int IDifferentialEquationSolver.VariablesCount
{
get
{
IMeasurements m = this;
return m.Count;
}
}
#endregion
#region IMeasurements Members
int IMeasurements.Count
{
get { return internalMeasurements.Length; }
}
IMeasure IMeasurements.this[int n]
{
get { return internalMeasurements[n]; }
}
void IMeasurements.UpdateMeasurements()
{
}
bool IMeasurements.IsUpdated
{
get
{
return isUpdated;
}
set
{
isUpdated = value;
}
}
#endregion
#region IDataConsumer Members
void IDataConsumer.Add(IMeasurements measurements)
{
this.measurements.Add(measurements);
}
void IDataConsumer.Remove(IMeasurements measurements)
{
this.measurements.Remove(measurements);
}
void IDataConsumer.UpdateChildrenData()
{
StaticDataPerformer.UpdateChildrenData(measurements);
}
int IDataConsumer.Count
{
get { return measurements.Count; }
}
IMeasurements IDataConsumer.this[int n]
{
get { return measurements[n]; }
}
void IDataConsumer.Reset()
{
StaticDataPerformer.Reset(this);
}
#endregion
#region IStarted Members
void IStarted.Start(double time)
{
Array.Copy(initialConditions, relativePosition, 3);
Array.Copy(initialConditions, 3, relativeVelocity, 0, 3);
Array.Copy(initialConditions, 6, relativeQuaternion, 0, 4);
Array.Copy(initialConditions, 10, omega, 0, 3);
Copy6DPosition();
Own.SetMatrix();
Update();
SetAliases();
}
#endregion
#region IVelocity Members
double[] IVelocity.Velocity
{
get { return velocity; }
}
/*
double[] IVelocity.RevativeVelocity
{
get { return relativeVelocity; }
}
*/
#endregion
#region IOrientation Members
double[] IOrientation.Quaternion
{
get { return quater; }
}
double[,] IOrientation.Matrix
{
get { return transitionMatrix; }
}
#endregion
#region IAngularVelocity Members
double[] IAngularVelocity.Omega
{
get { return omega; }
}
#endregion
#region IPostSetArrow Members
public override void PostSetArrow()
{
base.PostSetArrow();
post();
}
#endregion
#region Specific Members
/// <summary>
/// Sets parameters and aliases
/// </summary>
/// <param name="forcesString">Forces</param>
/// <param name="momentOfInertia">Moment of inertia</param>
/// <param name="state">State</param>
public void Set(string[] forcesString, string[] momentOfInertia, string[] state)
{
this.forcesString = forcesString;
this.momentOfInertia = momentOfInertia;
this.state = state;
post();
}
/// <summary>
/// String of forces
/// </summary>
public string[] Forces
{
get
{
return forcesString;
}
}
/// <summary>
/// String of moments
/// </summary>
public string[] Inretia
{
get
{
return momentOfInertia;
}
}
/// <summary>
/// String of state parameters
/// </summary>
public string[] State
{
get
{
return state;
}
}
/// <summary>
/// Initial conditions
/// </summary>
public double[] Initial
{
get
{
return initialConditions;
}
}
private void post()
{
measures = StaticDataPerformer.FindMeasures(this, forcesString, true);
inertia = StaticDataPerformer.FindMeasures(this, momentOfInertia, true);
aln = StaticDataPerformer.FindAliases(this, state, false);
}
/// <summary>
/// Sets aliases
/// </summary>
private void SetAliases()
{
ReferenceFrame own = Own;
IVelocity v = own as IVelocity;
IOrientation o = own as IOrientation;
IAngularVelocity av = own as IAngularVelocity;
StaticDataPerformer.SetAliases(aln, 0, own.Position, 0, 3);
StaticDataPerformer.SetAliases(aln, 3, v.Velocity, 0, 3);
StaticDataPerformer.SetAliases(aln, 6, o.Quaternion, 0, 4);
StaticDataPerformer.SetAliases(aln, 10, av.Omega, 0, 3);
StaticDataPerformer.SetAliases(aln, 13, relativeVelocity, 0, 3);
StaticDataPerformer.SetAliases(aln, 16, omega, 0, 3);
}
#region Measure Parametres
private object x()
{
return relativePosition[0];
}
private object y()
{
return relativePosition[1];
}
private object z()
{
return relativePosition[2];
}
private object Vx()
{
return relativeVelocity[0];
}
private object Vy()
{
return relativeVelocity[1];
}
private object Vz()
{
return relativeVelocity[2];
}
private object Q0()
{
return relativeQuaternion[0];
}
private object Q1()
{
return relativeQuaternion[1];
}
private object Q2()
{
return relativeQuaternion[2];
}
private object Q3()
{
return relativeQuaternion[3];
}
private object DQ0()
{
return quaternionDervation[0];
}
private object DQ1()
{
return quaternionDervation[1];
}
private object DQ2()
{
return quaternionDervation[2];
}
private object DQ3()
{
return quaternionDervation[3];
}
private object OMx()
{
return omega[0];
}
private object OMy()
{
return omega[1];
}
private object OMz()
{
return omega[2];
}
/* private object xd()
{
return result[0, 1];
}
private object yd()
{
return result[1, 1];
}
private object zd()
{
return result[2, 1];
}*/
private object Ax()
{
return linAccAbsolute[0];
}
private object Ay()
{
return linAccAbsolute[1];
}
private object Az()
{
return linAccAbsolute[2];
}
private object DOMx()
{
return epsRelative[0];
}
private object DOMy()
{
return epsRelative[1];
}
private object DOMz()
{
return epsRelative[2];
}
/* 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();
}
private void init()
{
Double a = 0;
internalMeasurements[0] = new MeasureDerivation(a, new Func<object>(x), new Measure(Vx, ""), "x");
internalMeasurements[1] = new MeasureDerivation(a, new Func<object>(y), new Measure(Vy, ""), "y");
internalMeasurements[2] = new MeasureDerivation(a, new Func<object>(z), new Measure(Vz, ""), "x");
internalMeasurements[3] = new MeasureDerivation(a, new Func<object>(Vx), new Measure(Ax, ""), "vx");
internalMeasurements[4] = new MeasureDerivation(a, new Func<object>(Vy), new Measure(Ay, ""), "vy");
internalMeasurements[5] = new MeasureDerivation(a, new Func<object>(Vz), new Measure(Az, ""), "vz");
internalMeasurements[6] = new MeasureDerivation(a, new Func<object>(Q0), new Measure(DQ0, ""), "q0");
internalMeasurements[7] = new MeasureDerivation(a, new Func<object>(Q1), new Measure(DQ1, ""), "q1");
internalMeasurements[8] = new MeasureDerivation(a, new Func<object>(Q2), new Measure(DQ2, ""), "q2");
internalMeasurements[9] = new MeasureDerivation(a, new Func<object>(Q3), new Measure(DQ3, ""), "q3");
internalMeasurements[10] = new MeasureDerivation(a, new Func<object>(OMx), new Measure(DOMx, ""), "omx");
internalMeasurements[11] = new MeasureDerivation(a, new Func<object>(OMy), new Measure(DOMy, ""), "omy");
internalMeasurements[12] = new MeasureDerivation(a, new Func<object>(OMz), new Measure(DOMz, ""), "omz");
}
#endregion
}
}
Submit an article or tip