示例#1
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);
                }
            }
        }
示例#2
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);
            }
        }