/// <summary>
        /// Solves equation
        /// </summary>
        protected virtual void Solve()
        {
            int k = 0;

            for (int i = 0; i < aggrWrappres.Length; i++)
            {
                AggregableWrapper           frame = aggrWrappres[i];
                IAggregableMechanicalObject agg   = frame.Aggregate;
                double[] intacc = agg.InternalAcceleration;
                int      l      = intacc.Length;
                Array.Copy(intacc, 0, vector, k, l);
                k += l;
            }
            if (matrix.GetLength(0) != 0)
            {
                CalculateLinkAccelerations();
                RealMatrixProcessor.RealMatrix.PlusEqual(vector, addAcceleration);
                for (int ln = 0; ln < links.Count; ln++)
                {
                    MechanicalAggregateLink     ml = links[ln];
                    IAggregableMechanicalObject s  = ml.SourceObject;
                    IAggregableMechanicalObject t  = ml.TargetObject;
                    int sc = ml.SourceConnection;
                    int tc = ml.TargetConnection;
                    int sn = numbers[s];
                    int tn = numbers[t];
                    Add(s, t, sc, tc, tn);
                    Add(t, s, tc, sc, sn);
                }
            }
        }
        void IDifferentialEquationSolver.CopyVariablesToSolver(int offset, double[] variables)
        {
            int n = offset;

            for (int i = 0; i < aggrWrappres.Length; i++)
            {
                AggregableWrapper           aw = aggrWrappres[i];
                IAggregableMechanicalObject ao = aw.Aggregate;
                double[] state = ao.State;
                for (int j = 0; j < state.Length; j++)
                {
                    double a = variables[n];
                    state[j] = a;
                    if (j < measures.Length)
                    {
                        AggergateDerivation der = measures[j] as AggergateDerivation;
                        der.Set(j, a);
                    }
                    ++n;
                }
                Motion6DAcceleratedFrame frame = aw.OwnFrame;
                IOrientation             or    = frame;
                Array.Copy(state, 6, or.Quaternion, 0, 4);
                IAngularVelocity w = frame;
                Array.Copy(state, 10, w.Omega, 0, 3);
            }
        }
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="aggregate">Root aggregate</param>
 protected MechanicalAggregateEquation(AggregableWrapper aggregate)
 {
     wrapper        = aggregate;
     this.aggregate = wrapper.Aggregate;
     PreInit();
     Init();
 }
 /// <summary>
 /// Creates aggregate equation
 /// </summary>
 /// <param name="aggregate">Root aggregate</param>
 /// <returns>Aggregate equation</returns>
 internal static MechanicalAggregateEquation CreateAggregateEquation(AggregableWrapper aggregate)
 {
     if (IsConstant(aggregate.Aggregate))
     {
         return(new RigidMechanicalAggregateEquation(aggregate));
     }
     return(new MechanicalAggregateEquation(aggregate));
 }
Esempio n. 5
0
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="aggregate">Root aggregate</param>
 internal RigidMechanicalAggregateEquation(AggregableWrapper aggregate)
     : base(aggregate)
 {
     CalculateMatrixes();
     if (matrix.GetLength(0) == 0)
     {
         return;
     }
     RealMatrixProcessor.RealMatrix.Invert(matrix, invertedMatrix);
     RealMatrixProcessor.RealMatrix.Multiply(forcesToAccelerations, invertedMatrix, finalMatrix);
 }
        /// <summary>
        /// Creates dictionary of aggregate
        /// </summary>
        /// <param name="aggregate">The aggregate</param>
        /// <param name="dic">The dictionary</param>
        /// <param name="deg">Degrees of freedom</param>
        /// <param name="add">Additional variables</param>
        /// <param name="acc">Akseleration number</param>
        private void CreateDictionary(AggregableWrapper aggregate,
                                      Dictionary <AggregableWrapper, Dictionary <AggregableWrapper, int[]> > dic,
                                      ref int deg, ref int add, ref int acc)
        {
            IAggregableMechanicalObject aggr = aggregate.Aggregate;

            numbers[aggr] = acc;
            int kd = add;
            int d0 = deg;

            deg += aggr.Dimension;
            list.Add(aggregate);
            lm.Add(aggr);
            acc += GetAcceleationDimension(aggr);
            Dictionary <IAggregableMechanicalObject, int[]> ch = aggr.Children;

            if (ch == null)
            {
                return;
            }
            if (ch.Count == 0)
            {
                return;
            }
            Dictionary <AggregableWrapper, int[]>         d   = new Dictionary <AggregableWrapper, int[]>();
            Dictionary <AggregableWrapper, int>           dn  = new Dictionary <AggregableWrapper, int>();
            Dictionary <IAggregableMechanicalObject, int> dna = new Dictionary <IAggregableMechanicalObject, int>();

            dictn[aggregate]            = dn;
            dictna[aggregate.Aggregate] = dna;
            dic[aggregate] = d;
            foreach (IAggregableMechanicalObject agg in ch.Keys)
            {
                AggregableWrapper aw = FindWrapper(agg);
                int[]             nc = ch[agg];
                d[aw]    = nc;
                dn[aw]   = add;
                dna[agg] = add;
                ++add;
                linkNumbers.Add(new int[] { kd, add, nc[1], nc[0], d0, deg });
                CreateDictionary(aw, dic, ref deg, ref add, ref acc);
            }
        }
        /// <summary>
        /// Finds wrapper of aggregable mechanical object
        /// </summary>
        /// <param name="obj">The aggregable mechanical object</param>
        /// <returns>The wrapper</returns>
        protected AggregableWrapper FindWrapper(IAggregableMechanicalObject obj)
        {
            IAssociatedObject             ao   = obj as IAssociatedObject;
            IObjectLabel                  l    = ao.Object as IObjectLabel;
            IDesktop                      d    = l.Desktop;
            IEnumerable <ICategoryObject> objs = d.CategoryObjects;

            foreach (ICategoryObject ob in objs)
            {
                if (ob is AggregableWrapper)
                {
                    AggregableWrapper wr = ob as AggregableWrapper;
                    if (wr.Aggregate == obj)
                    {
                        return(wr);
                    }
                }
            }
            return(null);
        }
        void IStateDoubleVariables.Set(double[] input, int offset, int length)
        {
            int n = 0;

            for (int i = 0; i < aggrWrappres.Length; i++)
            {
                AggregableWrapper           aw = aggrWrappres[i];
                IAggregableMechanicalObject ao = aw.Aggregate;
                double[] state = ao.State;
                for (int j = 0; j < state.Length; j++)
                {
                    state[j] = input[n + offset];
                    ++n;
                    if (n >= length)
                    {
                        return;
                    }
                }
            }
        }
        /// <summary>
        /// Root aggegates of desktop
        /// </summary>
        /// <param name="desktop">The desktop</param>
        /// <returns>Root aggregates</returns>
        public static ICollection <AggregableWrapper> GetRootAggregates(IEnumerable <object> desktop)
        {
            IEnumerable <object>     objs = desktop.GetObjectsAndArrows();
            List <AggregableWrapper> l    = new List <AggregableWrapper>();

            foreach (object o in objs)
            {
                if (!(o is AggregableWrapper))
                {
                    continue;
                }
                AggregableWrapper agg = o as AggregableWrapper;
                if (agg.Aggregate.Parent == null)
                {
                    if (!l.Contains(agg))
                    {
                        l.Add(agg);
                    }
                }
            }
            return(l);
        }
        void IDifferentialEquationSolver.CalculateDerivations()
        {
            Normalize(aggregate);
            foreach (AggregableWrapper aw in aggrWrappres)
            {
                IAggregableMechanicalObject obj = aw.Aggregate;
                if (obj is Diagram.UI.IUpdatableObject)
                {
                    Diagram.UI.IUpdatableObject uo = obj as Diagram.UI.IUpdatableObject;
                    if (uo.Update != null)
                    {
                        uo.Update();
                    }
                }
            }
            Solve();
            int n  = 0;
            int kv = 0;

            for (int i = 0; i < aggrWrappres.Length; i++)
            {
                AggregableWrapper           wrapper = aggrWrappres[i];
                IAggregableMechanicalObject agg     = wrapper.Aggregate;
                Motion6DAcceleratedFrame    frame   = wrapper.OwnFrame;
                IVelocity     vel   = frame;
                IAcceleration acc   = frame;
                IPosition     pos   = frame;
                double[]      state = agg.State;
                double[]      p     = pos.Position;
                double[]      v     = vel.Velocity;
                for (int j = 0; j < 3; j++)
                {
                    p[j] = state[j];
                    derivations[n + j, 0] = state[j];
                    double a = state[j + 3];
                    v[j] = a;
                    derivations[n + j, 1]     = a;
                    derivations[n + 3 + j, 0] = a;
                    derivations[n + 3 + j, 1] = vector[kv];
                    ++kv;
                }
                IOrientation or = frame;
                double[]     q  = or.Quaternion;
                for (int j = 0; j < 4; j++)
                {
                    double a = state[j + 6];
                    quater[j] = a;
                    q[j]      = a;
                }
                IAngularVelocity av = frame;
                double[]         om = av.Omega;
                for (int j = 0; j < 3; j++)
                {
                    double a = state[j + 10];
                    omega[j] = a;
                    om[j]    = a;
                }
                StaticExtensionVector3D.CalculateQuaternionDerivation(quater, omega, der, auxQuaternion);
                for (int j = 0; j < 4; j++)
                {
                    derivations[n + 6 + j, 0] = quater[j];
                    derivations[n + 6 + j, 1] = der[j];
                }
                for (int j = 0; j < 3; j++)
                {
                    derivations[n + 10 + j, 0] = omega[j];
                    derivations[n + 10 + j, 1] = vector[kv];
                    ++kv;
                }
                int kk  = n + 13;
                int stk = kk;
                int stv = 6;
                int sk  = 13;
                for (int j = 13; j < agg.Dimension; j++)
                {
                    derivations[kk, 0] = state[sk];
                    double a = state[sk + 1];
                    derivations[kk, 1] = a;
                    ++kk;
                    ++stk;
                    ++sk;
                    derivations[kk, 0] = a;
                    derivations[kk, 1] = vector[kv];
                    ++sk;
                    ++kv;
                    ++stv;
                    ++kk;
                    ++j;
                }
                n += agg.Dimension;
            }
        }