Exemplo n.º 1
0
        /// Subtracts this matrix and another matrix.
        /// Performance warning: a new object is created.
        public static ChState operator -(ChState a, ChMatrix matbis)
        {
            ChState result = new ChState(matbis.rows, a.integrable);

            result.matrix.MatrSub(result.matrix, matbis);
            return(result);
        }
Exemplo n.º 2
0
        // (override/implement interfaces for global state vectors, see ChPhysicsItem for comments.)
        public override void IntStateGather(int off_x,
                                            ref ChState x,
                                            int off_v,
                                            ref ChStateDelta v,
                                            ref double T)
        {
            int displ_x = off_x - this.offset_x;
            int displ_v = off_v - this.offset_w;

            for (int i = 0; i < bodylist.Count; i++)
            {
                if (bodylist[i].IsActive())
                {
                    bodylist[i].IntStateGather(displ_x + bodylist[i].GetOffset_x(), ref x, displ_v + bodylist[i].GetOffset_w(), ref v, ref T);
                }
            }
            for (int i = 0; i < linklist.Count; i++)
            {
                if (linklist[i].IsActive())
                {
                    linklist[i].IntStateGather(displ_x + linklist[i].GetOffset_x(), ref x, displ_v + linklist[i].GetOffset_w(), ref v, ref T);
                }
            }
            for (int i = 0; i < otherphysicslist.Count; i++)
            {
                otherphysicslist[i].IntStateGather(displ_x + otherphysicslist[i].GetOffset_x(), ref x, displ_v + otherphysicslist[i].GetOffset_w(), ref v, ref T);
            }
            T = GetChTime();
        }
Exemplo n.º 3
0
        /// Increments this matrix by another matrix, in place

        /*  public static ChState operator +(ChState state, ChMatrix<double> matbis) {
         *    state.MatrInc(matbis);
         *    return state;
         * }*/

        public static ChState operator +(ChState y, ChStateDelta Dy)
        {
            ChState tmp_y = new ChState(y);

            y.GetIntegrable().StateIncrement(ref y, tmp_y, Dy);
            return(y);
        }
Exemplo n.º 4
0
        public override void IntStateIncrement(int off_x,
                                               ref ChState x_new,
                                               ChState x,
                                               int off_v,
                                               ChStateDelta Dv)
        {
            int displ_x = off_x - (int)this.offset_x;
            int displ_v = off_v - (int)this.offset_w;

            for (int i = 0; i < bodylist.Count; i++)
            {
                if (bodylist[i].IsActive())
                {
                    bodylist[i].IntStateIncrement(displ_x + bodylist[i].GetOffset_x(), ref x_new, x, displ_v + bodylist[i].GetOffset_w(), Dv);
                }
            }
            for (int i = 0; i < linklist.Count; i++)
            {
                if (linklist[i].IsActive())
                {
                    linklist[i].IntStateIncrement(displ_x + linklist[i].GetOffset_x(), ref x_new, x, displ_v + linklist[i].GetOffset_w(), Dv);
                }
            }
            for (int i = 0; i < otherphysicslist.Count; i++)
            {
                otherphysicslist[i].IntStateIncrement(displ_x + otherphysicslist[i].GetOffset_x(), ref x_new, x, displ_v + otherphysicslist[i].GetOffset_w(), Dv);
            }
        }
Exemplo n.º 5
0
        public override void IntStateScatter(int off_x,
                                             ChState x,
                                             int off_v,
                                             ChStateDelta v,
                                             double T)
        {
            int displ_x = off_x - this.offset_x;
            int displ_v = off_v - this.offset_w;

            for (int i = 0; i < bodylist.Count; i++)
            {
                if (bodylist[i].IsActive())
                {
                    bodylist[i].IntStateScatter(displ_x + bodylist[i].GetOffset_x(), x, displ_v + bodylist[i].GetOffset_w(), v, T);
                }
            }
            for (int i = 0; i < linklist.Count; i++)
            {
                if (linklist[i].IsActive())
                {
                    linklist[i].IntStateScatter(displ_x + linklist[i].GetOffset_x(), x, displ_v + linklist[i].GetOffset_w(), v, T);
                }
            }
            for (int i = 0; i < otherphysicslist.Count; i++)
            {
                otherphysicslist[i].IntStateScatter(displ_x + otherphysicslist[i].GetOffset_x(), x, displ_v + otherphysicslist[i].GetOffset_w(), v, T);
            }
            SetChTime(T);

            // Note: all those IntStateScatter() above should call Update() automatically
            // for each object in the loop, therefore:
            // -do not call Update() on this.
            // -do not call ChPhysicsItem::IntStateScatter() -it calls this.Update() anyway-
            // because this would cause redundant updates.
        }
Exemplo n.º 6
0
 /// From item's state to global state vectors y={x,v}
 /// pasting the states at the specified offsets.
 public virtual void IntStateGather(int off_x,          //< offset in x state vector
                                    ref ChState x,      //< state vector, position part
                                    int off_v,          ///< offset in v state vector
                                    ref ChStateDelta v, ///< state vector, speed part
                                    ref double T        ///< time
                                    )
 {
 }
Exemplo n.º 7
0
 public override void IntStateScatter(int off_x,
                                      ChState x,
                                      int off_v,
                                      ChStateDelta v,
                                      double T)
 {
     // aux = x(off_x);
     aux_dt = v.matrix[off_v];
 }
Exemplo n.º 8
0
 public override void IntStateScatter(int off_x,
                                      ChState x,
                                      int off_v,
                                      ChStateDelta v,
                                      double T)
 {
     SetPos(x.matrix[off_x]);
     SetPos_dt(v.matrix[off_v]);
     update(T);
 }
Exemplo n.º 9
0
 public override void IntStateGather(int off_x,
                                     ref ChState x,
                                     int off_v,
                                     ref ChStateDelta v,
                                     ref double T)
 {
     x.matrix[off_x] = 0;  // aux;
     v.matrix[off_v] = aux_dt;
     T = GetChTime();
 }
Exemplo n.º 10
0
 /// From global state vectors y={x,v} to  item's state (and update)
 /// fetching the states at the specified offsets.
 public virtual void IntStateScatter(int off_x,      //< offset in x state vector
                                     ChState x,      ///< state vector, position part
                                     int off_v,      //< offset in v state vector
                                     ChStateDelta v, //< state vector, speed part
                                     double T        ///< time
                                     )
 {
     // Default behavior: even if no state is used, at least call Update()
     //update(T);
 }
Exemplo n.º 11
0
 /// Computes x_new = x + Dt , using vectors at specified offsets.
 /// By default, when DOF = DOF_w, it does just the sum, but in some cases (ex when using quaternions
 /// for rotations) it could do more complex stuff, and children classes might overload it.
 public virtual void IntStateIncrement(int off_x,         //< offset in x state vector
                                       ref ChState x_new, //< state vector, position part, incremented result
                                       ChState x,         //< state vector, initial position part
                                       int off_v,         //< offset in v state vector
                                       ChStateDelta Dv    //< state vector, increment
                                       )
 {
     for (int i = 0; i < GetDOF(); ++i)
     {
         x_new.matrix[off_x + i] = x.matrix[off_x + i] + Dv.matrix[off_v + i];
     }
 }
Exemplo n.º 12
0
        /// Compute all forces in a contiguous array.
        /// Used in finite-difference Jacobian approximation.
        public void CalculateQ(ChState stateA_x,              //< state positions for objA
                               ChStateDelta stateA_w,         //< state velocities for objA
                               ChState stateB_x,              //< state positions for objB
                               ChStateDelta stateB_w,         //< state velocities for objB
                               ChMaterialCompositeSMC mat,    //< composite material for contact pair
                               ref ChVectorDynamic <double> Q //< output generalized forces
                               )
        {
            ChBody oA = (this.objA as ChBody);
            ChBody oB = (this.objB as ChBody);

            // Express contact points in local frames.
            // We assume that these points remain fixed to their respective contactable objects.
            ChVector p1_loc = oA.GetCsysForCollisionModel().TransformPointParentToLocal(this.p1);
            ChVector p2_loc = oB.GetCsysForCollisionModel().TransformPointParentToLocal(this.p2);

            // Express the local points in global frame
            ChVector p1_abs = oA.GetContactPoint(p1_loc, stateA_x);
            ChVector p2_abs = oB.GetContactPoint(p2_loc, stateB_x);

            /*
             *  Note: while this can be somewhat justified for a ChBody, it will not work
             *        for a mesh vertex for instance...
             *
             * // Project the points onto the unperturbed normal line
             * p1_abs = this.p1 + Vdot(p1_abs - this.p1, this.normal) * this.normal;
             * p2_abs = this.p2 + Vdot(p2_abs - this.p2, this.normal) * this.normal;
             */

            // Calculate normal direction (expressed in global frame)
            ChVector normal_dir = (p1_abs - p2_abs).GetNormalized();

            // Calculate penetration depth
            double delta = (p1_abs - p2_abs).Length();

            // If the normal direction flipped sign, change sign of delta
            if (ChVector.Vdot(normal_dir, this.normal) < 0)
            {
                delta = -delta;
            }

            // Calculate velocity of contact points (expressed in global frame)
            ChVector vel1 = oA.GetContactPointSpeed(p1_loc, stateA_x, stateA_w);
            ChVector vel2 = oB.GetContactPointSpeed(p2_loc, stateB_x, stateB_w);

            // Compute the contact force.
            ChVector force = CalculateForce(delta, normal_dir, vel1, vel2, mat);

            // Compute and load the generalized contact forces.
            oA.ContactForceLoadQ(-force, p1_abs, stateA_x, ref Q, 0);
            oB.ContactForceLoadQ(force, p2_abs, stateB_x, ref Q, oA.ContactableGet_ndof_w());
        }
Exemplo n.º 13
0
        public override void IntStateIncrement(int off_x,
                                               ref ChState x_new,
                                               ChState x,
                                               int off_v,
                                               ChStateDelta Dv)
        {
            // First, inherit to parent class
            base.IntStateIncrement(off_x, ref x_new, x, off_v, Dv);

            if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
            {
                innershaft1.IntStateIncrement(off_x + 0, ref x_new, x, off_v + 0, Dv);
                innershaft2.IntStateIncrement(off_x + 1, ref x_new, x, off_v + 1, Dv);
            }
        }
Exemplo n.º 14
0
        // (override/implement interfaces for global state vectors, see ChPhysicsItem for comments.)
        // (beyond the base link implementations, it also have to
        // add the raint coming from the inner shaft etc.)
        public override void IntStateGather(int off_x,
                                            ref ChState x,
                                            int off_v,
                                            ref ChStateDelta v,
                                            ref double T)
        {
            // First, inherit to parent class
            base.IntStateGather(off_x, ref x, off_v, ref v, ref T);

            if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
            {
                innershaft1.IntStateGather(off_x + 0, ref x, off_v + 0, ref v, ref T);
                innershaft2.IntStateGather(off_x + 1, ref x, off_v + 1, ref v, ref T);
            }
        }
Exemplo n.º 15
0
        public override void IntStateScatter(int off_x,
                                             ChState x,
                                             int off_v,
                                             ChStateDelta v,
                                             double T)
        {
            // First, inherit to parent class
            base.IntStateScatter(off_x, x, off_v, v, T);

            if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
            {
                innershaft1.IntStateScatter(off_x + 0, x, off_v + 0, v, T);
                innershaft2.IntStateScatter(off_x + 1, x, off_v + 1, v, T);
            }
        }
Exemplo n.º 16
0
        /// Performs the static analysis,
        /// doing a linear solve.

        public override void StaticAnalysis()
        {
            ChIntegrableIIorder mintegrable = (ChIntegrableIIorder)this.integrable;

            // setup main vectors
            mintegrable.StateSetup(ref X, ref V, ref A);

            ChState                  Xnew = new ChState();
            ChStateDelta             Dx   = new ChStateDelta();
            ChVectorDynamic <double> R    = new ChVectorDynamic <double>();
            ChVectorDynamic <double> Qc   = new ChVectorDynamic <double>();
            double T = 0;

            // setup auxiliary vectors
            Dx.Reset(mintegrable.GetNcoords_v(), GetIntegrable());
            Xnew.Reset(mintegrable.GetNcoords_x(), mintegrable);
            R.Reset(mintegrable.GetNcoords_v());
            Qc.Reset(mintegrable.GetNconstr());
            L.Reset(mintegrable.GetNconstr());

            mintegrable.StateGather(ref X, ref V, ref T);  // state <- system

            // Set speed to zero
            V.matrix.FillElem(0);

            // Extrapolate a prediction as warm start
            Xnew = X;

            // use Newton Raphson iteration to solve implicit Euler for v_new
            //
            // [ - dF/dx    Cq' ] [ Dx  ] = [ f ]
            // [ Cq         0   ] [ L   ] = [ C ]

            for (int i = 0; i < this.GetMaxiters(); ++i)
            {
                mintegrable.StateScatter(Xnew, V, T);  // state -> system
                R.Reset();
                Qc.Reset();
                mintegrable.LoadResidual_F(ref R, 1.0);
                mintegrable.LoadConstraint_C(ref Qc, 1.0);

                double cfactor = ChMaths.ChMin(1.0, ((double)(i + 2) / (double)(incremental_steps + 1)));
                R  *= cfactor;
                Qc *= cfactor;

                //	GetLog()<< "Non-linear statics iteration=" << i << "  |R|=" << R.NormInf() << "  |Qc|=" << Qc.NormInf()
                //<< "\n";
                if ((R.matrix.NormInf() < this.GetTolerance()) && (Qc.matrix.NormInf() < this.GetTolerance()))
                {
                    break;
                }

                mintegrable.StateSolveCorrection(
                    ref Dx, ref L, R, Qc,
                    0,           // factor for  M
                    0,           // factor for  dF/dv
                    -1.0,        // factor for  dF/dx (the stiffness matrix)
                    Xnew, V, T,  // not needed here
                    false,       // do not StateScatter update to Xnew Vnew T+dt before computing correction
                    true         // force a call to the solver's Setup() function
                    );

                Xnew += Dx;
            }

            X = Xnew;

            mintegrable.StateScatter(X, V, T);    // state -> system
            mintegrable.StateScatterReactions(L); // -> system auxiliary data
        }
Exemplo n.º 17
0
 /// Copy constructor
 public ChState(ChState msource) : base(msource)
 {
     integrable = msource.integrable;
 }
Exemplo n.º 18
0
        /// Calculate Jacobian of generalized contact forces.
        public void CalculateJacobians(ChMaterialCompositeSMC mat)
        {
            // Compute a finite-difference approximations to the Jacobians of the contact forces and
            // load dQ/dx into m_Jac.m_K and dQ/dw into m_Jac.m_R.
            // Note that we only calculate these Jacobians whenever the contact force itself is calculated,
            // that is only once per step.  The Jacobian of generalized contact forces will therefore be
            // constant over the time step.

            ChBody oA = (this.objA as ChBody);
            ChBody oB = (this.objB as ChBody);

            // Get states for objA
            int          ndofA_x  = oA.ContactableGet_ndof_x();
            int          ndofA_w  = oA.ContactableGet_ndof_w();
            ChState      stateA_x = new ChState(ndofA_x, null);
            ChStateDelta stateA_w = new ChStateDelta(ndofA_w, null);

            oA.ContactableGetStateBlock_x(ref stateA_x);
            oA.ContactableGetStateBlock_w(ref stateA_w);

            // Get states for objB
            int          ndofB_x  = oB.ContactableGet_ndof_x();
            int          ndofB_w  = oB.ContactableGet_ndof_w();
            ChState      stateB_x = new ChState(ndofB_x, null);
            ChStateDelta stateB_w = new ChStateDelta(ndofB_w, null);

            oB.ContactableGetStateBlock_x(ref stateB_x);
            oB.ContactableGetStateBlock_w(ref stateB_w);

            // Compute Q at current state
            ChVectorDynamic <double> Q0 = new ChVectorDynamic <double>(ndofA_w + ndofB_w);

            CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q0);

            // Finite-difference approximation perturbation.
            // Note that ChState and ChStateDelta are set to 0 on construction.
            // To accommodate objects with quaternion states, use the method ContactableIncrementState while
            // calculating Jacobian columns corresponding to position states.
            double       perturbation = 1e-5;
            ChState      stateA_x1    = new ChState(ndofA_x, null);
            ChState      stateB_x1    = new ChState(ndofB_x, null);
            ChStateDelta prtrbA       = new ChStateDelta(ndofA_w, null);
            ChStateDelta prtrbB       = new ChStateDelta(ndofB_w, null);

            ChVectorDynamic <double> Q1      = new ChVectorDynamic <double>(ndofA_w + ndofB_w);
            ChVectorDynamic <double> Jcolumn = new ChVectorDynamic <double>(ndofA_w + ndofB_w);

            // Jacobian w.r.t. variables of objA
            for (int i = 0; i < ndofA_w; i++)
            {
                prtrbA.matrix[i] += perturbation;
                oA.ContactableIncrementState(stateA_x, prtrbA, ref stateA_x1);
                CalculateQ(stateA_x1, stateA_w, stateB_x, stateB_w, mat, ref Q1);
                prtrbA.matrix[i] -= perturbation;

                Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation);  // note sign change

                m_Jac.m_K.matrix.PasteMatrix(Jcolumn.matrix, 0, i);

                stateA_w.matrix[i] += perturbation;
                CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q1);
                stateA_w.matrix[i] -= perturbation;

                Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation);  // note sign change
                m_Jac.m_R.matrix.PasteMatrix(Jcolumn.matrix, 0, i);
            }

            // Jacobian w.r.t. variables of objB
            for (int i = 0; i < ndofB_w; i++)
            {
                prtrbB.matrix[i] += perturbation;
                oB.ContactableIncrementState(stateB_x, prtrbB, ref stateB_x1);
                CalculateQ(stateA_x, stateA_w, stateB_x1, stateB_w, mat, ref Q1);
                prtrbB.matrix[i] -= perturbation;

                Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation);  // note sign change
                m_Jac.m_K.matrix.PasteMatrix(Jcolumn.matrix, 0, ndofA_w + i);

                stateB_w.matrix[i] += perturbation;
                CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q1);
                stateB_w.matrix[i] -= perturbation;

                Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation);  // note sign change
                m_Jac.m_R.matrix.PasteMatrix(Jcolumn.matrix, 0, ndofA_w + i);
            }
        }
Exemplo n.º 19
0
        /// Perform the assembly analysis.
        /// Assembly is performed by satisfying constraints at a position, velocity, and acceleration levels.
        /// Assembly at position level involves solving a non-linear problem. Assembly at velocity level is
        /// performed by taking a small integration step. Consistent accelerations are obtained through
        /// finite differencing.
        public void AssemblyAnalysis(int action, double dt = 1e-7)
        {
            ChVectorDynamic <double> R  = new ChVectorDynamic <double>();
            ChVectorDynamic <double> Qc = new ChVectorDynamic <double>();
            double T = 0;

            // Set up main vectors
            integrable.StateSetup(ref X, ref V, ref A);

            if (action != 0 && AssemblyLevel.Enum.POSITION != 0)
            {
                ChStateDelta Dx = new ChStateDelta();

                for (int m_iter = 0; m_iter < max_assembly_iters; m_iter++)
                {
                    // Set up auxiliary vectors
                    Dx.Reset(integrable.GetNcoords_v(), GetIntegrable());
                    R.Reset(integrable.GetNcoords_v());
                    Qc.Reset(integrable.GetNconstr());
                    L.Reset(integrable.GetNconstr());

                    integrable.StateGather(ref X, ref V, ref T);  // state <- system

                    // Solve:
                    //
                    // [M          Cq' ] [ dx  ] = [ 0]
                    // [ Cq        0   ] [  l  ] = [ C]

                    integrable.LoadConstraint_C(ref Qc, 1.0);

                    integrable.StateSolveCorrection(
                        ref Dx, ref L, R, Qc,
                        1.0,      // factor for  M
                        0,        // factor for  dF/dv
                        0,        // factor for  dF/dx (the stiffness matrix)
                        X, V, T,  // not needed
                        false,    // do not StateScatter update to Xnew Vnew T+dt before computing correction
                        true      // force a call to the solver's Setup function
                        );

                    X += Dx;

                    integrable.StateScatter(X, V, T);  // state -> system
                }
            }

            if ((action != 0 & AssemblyLevel.Enum.VELOCITY != 0) || (action != 0 & AssemblyLevel.Enum.ACCELERATION != 0))
            {
                ChStateDelta Vold = new ChStateDelta();

                // setup auxiliary vectors
                Vold.Reset(integrable.GetNcoords_v(), GetIntegrable());
                R.Reset(integrable.GetNcoords_v());
                Qc.Reset(integrable.GetNconstr());
                L.Reset(integrable.GetNconstr());

                integrable.StateGather(ref X, ref V, ref T);  // state <- system

                Vold = V;

                // Perform a linearized semi-implicit Euler integration step
                //
                // [ M - dt*dF/dv - dt^2*dF/dx    Cq' ] [ v_new  ] = [ M*(v_old) + dt*f]
                // [ Cq                           0   ] [ -dt*l  ] = [ C/dt + Ct ]

                integrable.LoadResidual_F(ref R, dt);
                integrable.LoadResidual_Mv(ref R, V, 1.0);
                integrable.LoadConstraint_C(ref Qc, 1.0 / dt, false);
                integrable.LoadConstraint_Ct(ref Qc, 1.0);

                integrable.StateSolveCorrection(
                    ref V, ref L, R, Qc,
                    1.0,           // factor for  M
                    -dt,           // factor for  dF/dv
                    -dt * dt,      // factor for  dF/dx
                    X, V, T + dt,  // not needed
                    false,         // do not StateScatter update to Xnew Vnew T+dt before computing correction
                    true           // force a call to the solver's Setup() function
                    );

                integrable.StateScatter(X, V, T); // state -> system

                L *= (1.0 / dt);                  // Note it is not -(1.0/dt) because we assume StateSolveCorrection already flips sign of L

                if (action != 0 & AssemblyLevel.Enum.ACCELERATION != 0)
                {
                    integrable.StateScatterAcceleration(
                        (V - Vold) * (1 / dt));          // -> system auxiliary data (i.e acceleration as measure, fits DVI/MDI)

                    integrable.StateScatterReactions(L); // -> system auxiliary data
                }
            }
        }