double ChCollisionUtils::PointTriangleDistance(Vector B,
                                               Vector A1,
                                               Vector A2,
                                               Vector A3,
                                               double& mu,
                                               double& mv,
                                               int& is_into,
                                               Vector& Bprojected) {
    // defaults
    is_into = 0;
    mu = mv = -1;
    double mdistance = 10e22;

    Vector Dx, Dy, Dz, T1, T1p;

    Dx = Vsub(A2, A1);
    Dz = Vsub(A3, A1);
    Dy = Vcross(Dz, Dx);

    double dylen = Vlength(Dy);

    if (fabs(dylen) < EPS_TRIDEGEN)  // degenerate triangle
        return mdistance;

    Dy = Vmul(Dy, 1.0 / dylen);

    ChMatrix33<> mA;
    ChMatrix33<> mAi;
    mA.Set_A_axis(Dx, Dy, Dz);

    // invert triangle coordinate matrix -if singular matrix, was degenerate triangle-.
    if (fabs(mA.FastInvert(mAi)) < 0.000001)
        return mdistance;

    T1 = mAi.Matr_x_Vect(Vsub(B, A1));
    T1p = T1;
    T1p.y() = 0;
    mu = T1.x();
    mv = T1.z();
    mdistance = -T1.y();
    if (mu >= 0 && mv >= 0 && mv <= 1.0 - mu) {
        is_into = 1;
        Bprojected = Vadd(A1, mA.Matr_x_Vect(T1p));
    }

    return mdistance;
}
示例#2
0
void ChLinkGear::UpdateTime (double mytime)
{
    // First, inherit to parent class
    ChLinkLock::UpdateTime(mytime);

    // Move markers 1 and 2 to align them as gear teeth

    ChMatrix33<> ma1;
    ChMatrix33<> ma2;
    ChMatrix33<> mrotma;
    ChMatrix33<> marot_beta;
    Vector mx;
    Vector my;
    Vector mz;
	Vector mr;
    Vector mmark1;
    Vector mmark2;
    Vector lastX;
    Vector vrota;
    Coordsys newmarkpos;

	ChFrame<double> abs_shaft1;
	ChFrame<double> abs_shaft2;

	((ChFrame<double>*)Body1)->TrasformLocalToParent(local_shaft1, abs_shaft1);
	((ChFrame<double>*)Body2)->TrasformLocalToParent(local_shaft2, abs_shaft2);

    Vector vbdist = Vsub(Get_shaft_pos1(),
                          Get_shaft_pos2());
    Vector Trad1 = Vnorm(Vcross(Get_shaft_dir1(), Vnorm(Vcross(Get_shaft_dir1(),vbdist))));
    Vector Trad2 = Vnorm(Vcross(Vnorm(Vcross(Get_shaft_dir2(),vbdist)), Get_shaft_dir2()));

	double dist = Vlenght(vbdist);
    

        // compute actual rotation of the two wheels (relative to truss).
    Vector md1 = abs_shaft1.GetA()->MatrT_x_Vect(-vbdist);
    md1.z = 0;  md1 = Vnorm (md1);
    Vector md2 = abs_shaft2.GetA()->MatrT_x_Vect(-vbdist);
    md2.z = 0;  md2 = Vnorm (md2);

	double periodic_a1 = ChAtan2(md1.x, md1.y);
	double periodic_a2 = ChAtan2(md2.x, md2.y);
	double old_a1 = a1; 
	double old_a2 = a2;
	double turns_a1 = floor (old_a1 / CH_C_2PI);
	double turns_a2 = floor (old_a2 / CH_C_2PI);
	double a1U = turns_a1 * CH_C_2PI + periodic_a1 + CH_C_2PI;
	double a1M = turns_a1 * CH_C_2PI + periodic_a1;
	double a1L = turns_a1 * CH_C_2PI + periodic_a1 - CH_C_2PI;
	a1 = a1M;
	if (fabs(a1U - old_a1) < fabs(a1M - old_a1))
		a1 = a1U;
	if (fabs(a1L - a1) < fabs(a1M - a1))
		a1 = a1L;
	double a2U = turns_a2 * CH_C_2PI + periodic_a2 + CH_C_2PI;
	double a2M = turns_a2 * CH_C_2PI + periodic_a2;
	double a2L = turns_a2 * CH_C_2PI + periodic_a2 - CH_C_2PI;
	a2 = a2M;
	if (fabs(a2U - old_a2) < fabs(a2M - old_a2))
		a2 = a2U;
	if (fabs(a2L - a2) < fabs(a2M - a2))
		a2 = a2L;


        // compute new markers coordsystem alignment
    my = Vnorm (vbdist);
    mz = Get_shaft_dir1();
    mx = Vnorm(Vcross (my, mz));
	mr = Vnorm(Vcross (mz, mx));
    mz = Vnorm(Vcross (mx, my));
	ChVector<> mz2, mx2, mr2, my2;
	my2 = my;
	mz2 = Get_shaft_dir2();
	mx2 = Vnorm(Vcross (my2, mz2));
	mr2 = Vnorm(Vcross (mz2, mx2));

    ma1.Set_A_axis(mx,my,mz);

        // rotate csys because of beta
    vrota.x = 0.0;  vrota.y = this->beta;  vrota.z = 0.0;
    mrotma.Set_A_Rxyz(vrota);
    marot_beta.MatrMultiply(ma1, mrotma);
        // rotate csys because of alpha
    vrota.x = 0.0;  vrota.y = 0.0;  vrota.z = this->alpha;
    if (react_force.x < 0)  vrota.z =  this->alpha;
    else                    vrota.z = -this->alpha;
    mrotma.Set_A_Rxyz(vrota);
    ma1.MatrMultiply(marot_beta, mrotma);

    ma2.CopyFromMatrix(ma1);

		// is a bevel gear?
	double be = acos(Vdot(Get_shaft_dir1(), Get_shaft_dir2()));
	bool is_bevel= true;
	if (fabs( Vdot(Get_shaft_dir1(), Get_shaft_dir2()) )>0.96)
		is_bevel = false;

        // compute wheel radii
        // so that:
        //            w2 = - tau * w1
	if (!is_bevel)
	{
		double pardist =  Vdot(mr, vbdist);
		double inv_tau = 1.0/tau;
		if  (!this->epicyclic)
		{
			r2 = pardist - pardist / (inv_tau + 1.0);
		}
		else
		{
			r2 = pardist - (tau * pardist)/(tau-1.0);
		}
		r1 = r2*tau;	}
	else
	{
		double gamma2;
		if  (!this->epicyclic)
		{
			gamma2 = be/(tau + 1.0);
		}
		else
		{
			gamma2 = be/(-tau + 1.0);
		}
	   double al = CH_C_PI - acos (Vdot(Get_shaft_dir2(), my));
		double te = CH_C_PI - al - be;
		double fd = sin(te) * (dist/sin(be));
		r2 = fd * tan(gamma2);
		r1 = r2*tau;
	}

        // compute markers positions, supposing they
        // stay on the ideal wheel contact point
	mmark1 = Vadd(Get_shaft_pos2(), Vmul(mr2, r2));
    mmark2 = mmark1;
    contact_pt = mmark1;

        // correct marker 1 position if phasing is not correct
    if (this->checkphase)
    {
		double realtau = tau; 
		if (this->epicyclic) 
			realtau = -tau;
        double m_delta;
        m_delta = - (a2/realtau) - a1 - phase;

        if (m_delta> CH_C_PI) m_delta -= (CH_C_2PI);		 // range -180..+180 is better than 0...360
        if (m_delta> (CH_C_PI/4.0)) m_delta = (CH_C_PI/4.0); // phase correction only in +/- 45°
        if (m_delta<-(CH_C_PI/4.0)) m_delta =-(CH_C_PI/4.0);

        vrota.x = vrota.y = 0.0;  vrota.z = - m_delta;
        mrotma.Set_A_Rxyz(vrota);   // rotate about Z of shaft to correct
        mmark1 = abs_shaft1.GetA()->MatrT_x_Vect(Vsub(mmark1,  Get_shaft_pos1() ));
        mmark1 = mrotma.Matr_x_Vect(mmark1);
        mmark1 = Vadd (abs_shaft1.GetA()->Matr_x_Vect(mmark1), Get_shaft_pos1() );
    }
		// Move Shaft 1 along its direction if not aligned to wheel
	double offset =  Vdot (this->Get_shaft_dir1(), (contact_pt - this->Get_shaft_pos1()) );
	ChVector<> moff = this->Get_shaft_dir1() * offset;
	if (fabs (offset) > 0.0001)
		this->local_shaft1.SetPos( local_shaft1.GetPos() + Body1->Dir_World2Body(&moff) );
		

            // ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
    marker2->SetMotionType(ChMarker::M_MOTION_EXTERNAL);
    marker1->SetMotionType(ChMarker::M_MOTION_EXTERNAL);

        // move marker1 in proper positions
    newmarkpos.pos = mmark1;
    newmarkpos.rot = ma1.Get_A_quaternion();
    marker1->Impose_Abs_Coord(newmarkpos);        //move marker1 into teeth position
        // move marker2 in proper positions
    newmarkpos.pos = mmark2;
    newmarkpos.rot = ma2.Get_A_quaternion();
    marker2->Impose_Abs_Coord(newmarkpos);        //move marker2 into teeth position


        // imposed relative positions/speeds
    deltaC.pos = VNULL;
    deltaC_dt.pos = VNULL;
    deltaC_dtdt.pos = VNULL;

    deltaC.rot = QUNIT;             // no relative rotations imposed!
    deltaC_dt.rot = QNULL;
    deltaC_dtdt.rot = QNULL;

}