Ejemplo n.º 1
0
        public override void ContIntLoadConstraint_C(int off_L,
                                                     ref ChVectorDynamic <double> Qc,
                                                     double c,
                                                     bool do_clamp,
                                                     double recovery_clamp
                                                     )
        {
            bool bounced = false;

            // Elastic Restitution model (use simple Newton model with coefficient e=v(+)/v(-))
            // Note that this works only if the two connected items are two ChBody.

            if (this.objA != null && this.objB != null)
            {
                var oA = (ChBody)(object)this.objA;
                var oB = (ChBody)(object)this.objB;
                if (this.restitution != 0)
                {
                    // compute normal rebounce speed
                    ChVector V1_w        = oA.GetContactPointSpeed(this.p1);
                    ChVector V2_w        = oB.GetContactPointSpeed(this.p2);
                    ChVector Vrel_w      = V2_w - V1_w;
                    ChVector Vrel_cplane = this.contact_plane.MatrT_x_Vect(Vrel_w);

                    double h = this.container.GetSystem().GetStep();  // = 1.0 / c;  // not all steppers have c = 1/h

                    double neg_rebounce_speed = Vrel_cplane.x * this.restitution;
                    if (neg_rebounce_speed < -this.container.GetSystem().GetMinBounceSpeed())
                    {
                        if (this.norm_dist + neg_rebounce_speed * h < 0)
                        {
                            // CASE: BOUNCE
                            bounced           = true;
                            Qc.matrix[off_L] += neg_rebounce_speed;
                        }
                    }
                }
            }

            if (!bounced)
            {
                // CASE: SETTLE (most often, and also default if two colliding items are not two ChBody)

                if (this.compliance != 0)
                {
                    double h = 1.0 / c;                        // was: this->container->GetSystem()->GetStep(); note not all steppers have c = 1/h

                    double alpha    = this.dampingf;           // [R]=alpha*[K]
                    double inv_hpa  = 1.0 / (h + alpha);       // 1/(h+a)
                    double inv_hhpa = 1.0 / (h * (h + alpha)); // 1/(h*(h+a))

                    //***TODO*** move to KRMmatricesLoad() the following, and only for !bounced case
                    Nx.Set_cfm_i((inv_hhpa) * this.compliance);
                    Tu.Set_cfm_i((inv_hhpa) * this.complianceT);
                    Tv.Set_cfm_i((inv_hhpa) * this.complianceT);

                    double qc = inv_hpa * this.norm_dist;  //***TODO*** see how to move this in KRMmatricesLoad()

                    // Note: clamping of Qc in case of compliance is questionable: it does not limit only the outbound
                    // speed, but also the reaction, so it might allow longer 'sinking' not related to the real compliance.
                    // I.e. If clamping kicks in (when using large timesteps and low compliance), it acts as a numerical damping.
                    if (do_clamp)
                    {
                        qc = ChMaths.ChMax(qc, -recovery_clamp);
                    }

                    Qc.matrix[off_L] += qc;
                }
                else
                {
                    if (do_clamp)
                    {
                        if (this.Nx.Constraint2TuplesNall.GetCohesion() != 0)
                        {
                            Qc.matrix[off_L] += ChMaths.ChMin(0.0, ChMaths.ChMax(c * this.norm_dist, -recovery_clamp));
                        }
                        else
                        {
                            Qc.matrix[off_L] += ChMaths.ChMax(c * this.norm_dist, -recovery_clamp);
                        }
                    }
                    else
                    {
                        Qc.matrix[off_L] += c * this.norm_dist;
                    }
                }
            }
        }