Ejemplo n.º 1
0
        /// Initialization based on passing two vectors (point + dir) on the
        /// two bodies, they will represent the X axes of the two frames (Y and Z will
        /// be built from the X vector via Gram Schmidt orthonormalization).
        /// Use the other ChLinkMateGeneric::Initialize() if you want to set the two frames directly.
        public virtual void Initialize(ChBodyFrame mbody1,    //< first body to link
                                       ChBodyFrame mbody2,    //< second body to link
                                       bool pos_are_relative, //< true: following pos. are relative to bodies.
                                       ChVector mpt1,         //< origin of slave frame 1, for 1st body (rel. or abs., see flag above)
                                       ChVector mpt2,         //< origin of master frame 2, for 2nd body (rel. or abs., see flag above)
                                       ChVector mnorm1,       //< X axis of slave plane, for 1st body (rel. or abs., see flag above)
                                       ChVector mnorm2        //< X axis of master plane, for 2nd body (rel. or abs., see flag above)
                                       )
        {
            Debug.Assert(mbody1 != mbody2);

            this.Body1 = mbody1;
            this.Body2 = mbody2;
            // this.SetSystem(mbody1.GetSystem());

            this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());

            ChVector            mx   = new ChVector(0, 0, 0);
            ChVector            my   = new ChVector(0, 0, 0);
            ChVector            mz   = new ChVector(0, 0, 0);
            ChVector            mN   = new ChVector(0, 0, 0);
            ChMatrix33 <double> mrot = new ChMatrix33 <double>();

            ChFrame <double> mfr1 = new ChFrame <double>();
            ChFrame <double> mfr2 = new ChFrame <double>();

            if (pos_are_relative)
            {
                mN = mnorm1;
                mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0));
                mrot.Set_A_axis(mx, my, mz);
                mfr1.SetRot(mrot);
                mfr1.SetPos(mpt1);

                mN = mnorm2;
                mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0));
                mrot.Set_A_axis(mx, my, mz);
                mfr2.SetRot(mrot);
                mfr2.SetPos(mpt2);
            }
            else
            {
                ChVector temp = ChVector.VECT_Z;
                // from abs to body-rel
                mN = this.Body1.TransformDirectionParentToLocal(mnorm1);
                mN.DirToDxDyDz(ref mx, ref my, ref mz, temp);
                mrot.Set_A_axis(mx, my, mz);
                mfr1.SetRot(mrot);
                mfr1.SetPos(this.Body1.TransformPointParentToLocal(mpt1));

                mN = this.Body2.TransformDirectionParentToLocal(mnorm2);
                mN.DirToDxDyDz(ref mx, ref my, ref mz, temp);
                mrot.Set_A_axis(mx, my, mz);
                mfr2.SetRot(mrot);
                mfr2.SetPos(this.Body2.TransformPointParentToLocal(mpt2));
            }

            this.frame1 = mfr1;
            this.frame2 = mfr2;
        }
Ejemplo n.º 2
0
        /// Specialized initialization for generic mate, given the two bodies to be connected, the
        /// positions of the two frames to connect on the bodies (each expressed
        /// in body or abs. coordinates).
        public virtual void Initialize(ChBodyFrame mbody1,     //< first body to link
                                       ChBodyFrame mbody2,     /// second body to link
                                       bool pos_are_relative,  //< true: following pos. are relative to bodies.
                                       ChFrame <double> mpos1, //< mate frame (slave), for 1st body (rel. or abs., see flag above)
                                       ChFrame <double> mpos2  //< mate frame (master), for 2nd body (rel. or abs., see flag above)
                                       )
        {
            Debug.Assert(mbody1 != mbody2);

            this.Body1 = mbody1;
            this.Body2 = mbody2;
            // this.SetSystem(mbody1.GetSystem());

            this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());

            if (pos_are_relative)
            {
                this.frame1 = mpos1;
                this.frame2 = mpos2;
            }
            else
            {
                // from abs to body-rel
                (this.Body1).TransformParentToLocal(mpos1, this.frame1);
                this.Body2.TransformParentToLocal(mpos2, this.frame2);
            }
        }
Ejemplo n.º 3
0
        //
        // STATE FUNCTIONS
        //

        /// Adds force to residual R, as R*= F*c
        /// NOTE: here the off offset in R is NOT used because add F at the TWO offsets of the two connected bodies,
        /// so it is assumed that offsets for Body1 and Body2 variables have been already set properly!
        public override void IntLoadResidual_F(int off, ref ChVectorDynamic <double> R, double c)
        {
            if (Body1 == null || Body2 == null)
            {
                return;
            }

            ChVector mbody_force  = new ChVector(0, 0, 0);
            ChVector mbody_torque = new ChVector(0, 0, 0);

            if (ChVector.Vnotnull(C_force))
            {
                ChVector m_abs_force = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_force));

                if (Body2.Variables().IsActive())
                {
                    Body2.To_abs_forcetorque(m_abs_force,
                                             marker1.GetAbsCoord().pos,          // absolute application point is always marker1
                                             false,                              // from abs. space
                                             ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
                    R.matrix.PasteSumVector(mbody_force * -c, Body2.Variables().GetOffset(), 0);
                    R.matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(mbody_torque) * -c,
                                            Body2.Variables().GetOffset() + 3, 0);
                }

                if (Body1.Variables().IsActive())
                {
                    Body1.To_abs_forcetorque(m_abs_force,
                                             marker1.GetAbsCoord().pos,          // absolute application point is always marker1
                                             false,                              // from abs. space
                                             ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
                    R.matrix.PasteSumVector(mbody_force * c, Body1.Variables().GetOffset(), 0);
                    R.matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(mbody_torque) * c,
                                            Body1.Variables().GetOffset() + 3, 0);
                }
            }
            if (ChVector.Vnotnull(C_torque))
            {
                ChVector m_abs_torque = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_torque));
                // load torques in 'fb' vector accumulator of body variables (torques in local coords)
                if (Body1.Variables().IsActive())
                {
                    R.matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * c,
                                            Body1.Variables().GetOffset() + 3, 0);
                }
                if (Body2.Variables().IsActive())
                {
                    R.matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -c,
                                            Body2.Variables().GetOffset() + 3, 0);
                }
            }
        }
Ejemplo n.º 4
0
        /// overwrites inherited implementation of this method
        public override void SetUpMarkers(ChMarker mark1, ChMarker mark2)
        {
            base.SetUpMarkers(mark1, mark2);

            // could the line below be:     assert(this.Body1 && this.Body2); ?
            if (this.Body1 != null && this.Body2 != null)
            {
                ((ChLinkMaskLF)this.mask).SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
                // This is needed because only if all constraints in mask are now active, and C,Ct,etc.
                // matrices must be allocated accordingly, otherwise are null.
                DestroyLink();
                BuildLink();
            }
        }
Ejemplo n.º 5
0
        //
        // SOLVER INTERFACE (OLD)
        //
        public override void ConstraintsFbLoadForces(double factor = 1)
        {
            // compute instant torque
            double mT = m_func.Get_y(this.GetChTime());

            ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));

            ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));

            ChVector m_abs_torque = aframe2.GetA().Matr_x_Vect(new ChVector(0, 0, mT));

            Body2.Variables().Get_fb().matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -factor, 3, 0);

            Body1.Variables().Get_fb().matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * factor, 3, 0);
        }
Ejemplo n.º 6
0
        //
        // STATE FUNCTIONS
        //
        public override void IntLoadResidual_F(int off, ref ChVectorDynamic <double> R, double c)
        {
            // compute instant torque
            double mT = m_func.Get_y(this.GetChTime());

            ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));

            ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));

            ChVector m_abs_torque = aframe2.GetA().Matr_x_Vect(new ChVector(0, 0, mT));

            if (Body2.Variables().IsActive())
            {
                R.matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -c, Body2.Variables().GetOffset() + 3,
                                        0);
            }

            if (Body1.Variables().IsActive())
            {
                R.matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * c, Body1.Variables().GetOffset() + 3,
                                        0);
            }
        }
Ejemplo n.º 7
0
        //
        // SOLVER INTERFACE
        //

        /// Overrides the empty behaviour of the parent ChLink implementation, which
        /// does not consider any user-imposed force between the two bodies.
        /// It adds the current link-forces, if any, (caused by springs, etc.) to the 'fb' vectors
        /// of the ChVariables referenced by encapsulated ChConstraints.
        /// In details, it adds the effect caused by C_force and C_torque.
        /// Both C_force and C_torque these forces are considered expressed in the
        /// reference coordsystem of marker2 (the MAIN marker),
        /// and their application point is the origin of marker1 (the SLAVE marker).
        public override void ConstraintsFbLoadForces(double factor = 1)
        {
            if (Body1 == null || Body2 == null)
            {
                return;
            }

            ChVector mbody_force  = new ChVector(0, 0, 0);
            ChVector mbody_torque = new ChVector(0, 0, 0);

            if (ChVector.Vnotnull(C_force))
            {
                ChVector m_abs_force = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_force));
                Body2.To_abs_forcetorque(m_abs_force,
                                         marker1.GetAbsCoord().pos,          // absolute application point is always marker1
                                         false,                              // from abs. space
                                         ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
                Body2.Variables().Get_fb().matrix.PasteSumVector(mbody_force * -factor, 0, 0);
                Body2.Variables().Get_fb().matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(mbody_torque) * -factor, 3,
                                                                 0);

                Body1.To_abs_forcetorque(m_abs_force,
                                         marker1.GetAbsCoord().pos,          // absolute application point is always marker1
                                         false,                              // from abs. space
                                         ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
                Body1.Variables().Get_fb().matrix.PasteSumVector(mbody_force * factor, 0, 0);
                Body1.Variables().Get_fb().matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(mbody_torque) * factor, 3, 0);
            }
            if (ChVector.Vnotnull(C_torque))
            {
                ChVector m_abs_torque = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_torque));
                // load torques in 'fb' vector accumulator of body variables (torques in local coords)
                Body1.Variables().Get_fb().matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * factor, 3, 0);
                Body2.Variables().Get_fb().matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -factor, 3,
                                                                 0);
            }
        }
Ejemplo n.º 8
0
        //
        // UPDATING FUNCTIONS
        //

        /// Override _all_ time, jacobian etc. updating.
        public override void update(double mytime, bool update_assets = true)
        {
            // Inherit time changes of parent class (ChLink), basically doing nothing :)
            base.update(mytime, update_assets);

            if (this.Body1 != null && this.Body2 != null)
            {
                this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());

                ChFrame <double> aframe = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));

                ChVector         p1_abs  = aframe.GetPos();
                ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));

                ChVector         p2_abs = aframe2.GetPos();
                ChFrame <double> bframe = new ChFrame <double>();
                (this.Body2).TransformParentToLocal(aframe, bframe);
                this.frame2.TransformParentToLocal(bframe, aframe);
                // Now 'aframe' contains the position/rotation of frame 1 respect to frame 2, in frame 2 coords.
                //***TODO*** check if it is faster to do   aframe2.TransformParentToLocal(aframe, bframe); instead of two transforms above

                ChMatrix33 <double> Jx1    = new ChMatrix33 <double>(0);
                ChMatrix33 <double> Jx2    = new ChMatrix33 <double>(0);
                ChMatrix33 <double> Jr1    = new ChMatrix33 <double>(0);
                ChMatrix33 <double> Jr2    = new ChMatrix33 <double>(0);
                ChMatrix33 <double> Jw1    = new ChMatrix33 <double>(0);
                ChMatrix33 <double> Jw2    = new ChMatrix33 <double>(0);
                ChMatrix33 <double> mtempM = new ChMatrix33 <double>(0);
                ChMatrix33 <double> mtempQ = new ChMatrix33 <double>(0);

                ChMatrix33 <double> abs_plane = new ChMatrix33 <double>(0);
                abs_plane.nm.matrix.MatrMultiply(Body2.GetA().nm.matrix, frame2.GetA().nm.matrix);

                Jx1.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix);
                Jx2.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix);
                Jx2.nm.matrix.MatrNeg();

                Jw1.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body1.GetA().nm.matrix);
                Jw2.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body2.GetA().nm.matrix);

                mtempM.Set_X_matrix(frame1.GetPos());
                Jr1.nm.matrix.MatrMultiply(Jw1.nm.matrix, mtempM.nm.matrix);
                Jr1.nm.matrix.MatrNeg();

                mtempM.Set_X_matrix(frame2.GetPos());
                Jr2.nm.matrix.MatrMultiply(Jw2.nm.matrix, mtempM.nm.matrix);

                ChVector p2p1_base2 = (Body2.GetA()).MatrT_x_Vect(ChVector.Vsub(p1_abs, p2_abs));
                mtempM.Set_X_matrix(p2p1_base2);
                mtempQ.nm.matrix.MatrTMultiply(frame2.GetA().nm.matrix, mtempM.nm.matrix);
                Jr2.nm.matrix.MatrInc(mtempQ.nm.matrix);

                Jw2.nm.matrix.MatrNeg();

                // Premultiply by Jw1 and Jw2 by  0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion.
                // For small misalignment this effect is almost insignificant cause [Fp(q_resid)]=[I],
                // but otherwise it is needed (if you want to use the stabilization term - if not, you can live without).
                mtempM.Set_X_matrix((aframe.GetRot().GetVector()) * 0.5);
                mtempM.nm.matrix[0, 0] = 0.5 * aframe.GetRot().e0;
                mtempM.nm.matrix[1, 1] = 0.5 * aframe.GetRot().e0;
                mtempM.nm.matrix[2, 2] = 0.5 * aframe.GetRot().e0;
                mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw1.nm.matrix);
                Jw1 = mtempQ;
                mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw2.nm.matrix);
                Jw2 = mtempQ;

                int nc = 0;

                if (c_x)
                {
                    this.C.matrix.ElementN(nc) = aframe.GetPos().x;
                    this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 0, 0, 1, 3, 0, 0);
                    this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 0, 0, 1, 3, 0, 3);
                    this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 0, 0, 1, 3, 0, 0);
                    this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 0, 0, 1, 3, 0, 3);
                    nc++;
                }
                if (c_y)
                {
                    this.C.matrix.ElementN(nc) = aframe.GetPos().y;
                    this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 1, 0, 1, 3, 0, 0);
                    this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 1, 0, 1, 3, 0, 3);
                    this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 1, 0, 1, 3, 0, 0);
                    this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 1, 0, 1, 3, 0, 3);
                    nc++;
                }
                if (c_z)
                {
                    this.C.matrix.ElementN(nc) = aframe.GetPos().z;
                    this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 2, 0, 1, 3, 0, 0);
                    this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 2, 0, 1, 3, 0, 3);
                    this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 2, 0, 1, 3, 0, 0);
                    this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 2, 0, 1, 3, 0, 3);
                    nc++;
                }
                if (c_rx)
                {
                    this.C.matrix.ElementN(nc) = aframe.GetRot().e1;
                    this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
                    this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
                    this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3);
                    this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3);
                    nc++;
                }
                if (c_ry)
                {
                    this.C.matrix.ElementN(nc) = aframe.GetRot().e2;
                    this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
                    this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
                    this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3);
                    this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3);
                    nc++;
                }
                if (c_rz)
                {
                    this.C.matrix.ElementN(nc) = aframe.GetRot().e3;
                    this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
                    this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
                    this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3);
                    this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3);
                    nc++;
                }
            }
        }