Beispiel #1
0
void ChLinkDistance::Update (double mytime)
{
    // Inherit time changes of parent class (ChLink), basically doing nothing :)
    ChLink::UpdateTime(mytime);

		// compute jacobians
	ChVector<> AbsDist = Body1->Point_Body2World(&pos1)-Body2->Point_Body2World(&pos2);
	curr_dist          = AbsDist.Length();
	ChVector<> D2abs   = Vnorm(AbsDist);
	ChVector<> D2relB  = Body2->Dir_World2Body(&D2abs);
	ChVector<> D2relA  = Body1->Dir_World2Body(&D2abs);

	ChVector<> CqAx =  D2abs;
	ChVector<> CqBx = -D2abs;
	
	ChVector<> CqAr = -Vcross(D2relA,pos1);
	ChVector<> CqBr =  Vcross(D2relB,pos2);

	Cx.Get_Cq_a()->ElementN(0)=(float)CqAx.x;
	Cx.Get_Cq_a()->ElementN(1)=(float)CqAx.y;
	Cx.Get_Cq_a()->ElementN(2)=(float)CqAx.z;
	Cx.Get_Cq_a()->ElementN(3)=(float)CqAr.x;
	Cx.Get_Cq_a()->ElementN(4)=(float)CqAr.y;
	Cx.Get_Cq_a()->ElementN(5)=(float)CqAr.z;

	Cx.Get_Cq_b()->ElementN(0)=(float)CqBx.x;
	Cx.Get_Cq_b()->ElementN(1)=(float)CqBx.y;
	Cx.Get_Cq_b()->ElementN(2)=(float)CqBx.z;
	Cx.Get_Cq_b()->ElementN(3)=(float)CqBr.x;
	Cx.Get_Cq_b()->ElementN(4)=(float)CqBr.y;
	Cx.Get_Cq_b()->ElementN(5)=(float)CqBr.z;

	//***TO DO***  C_dt? C_dtdt? (may be never used..)
}
Beispiel #2
0
vector inoutattractor(vector v,vector a,float strength,int period)
{
  if (Vequal(v,a))
    return V(0,0);
  else
    return Vmult(strength*(1-radius(Vsub(v,a)))*sin(frame*2*pi/period),Vnorm(Vsub(v,a)));
}
Beispiel #3
0
void ChLinkDistance::Update(double mytime, bool update_assets) {
    // Inherit time changes of parent class (ChLink), basically doing nothing :)
    ChLink::Update(mytime, update_assets);

    // compute jacobians
    ChVector<> AbsDist = Body1->TransformPointLocalToParent(pos1) - Body2->TransformPointLocalToParent(pos2);
    curr_dist = AbsDist.Length();
    ChVector<> D2abs = Vnorm(AbsDist);
    ChVector<> D2relB = Body2->TransformDirectionParentToLocal(D2abs);
    ChVector<> D2relA = Body1->TransformDirectionParentToLocal(D2abs);

    ChVector<> CqAx = D2abs;
    ChVector<> CqBx = -D2abs;

    ChVector<> CqAr = -Vcross(D2relA, pos1);
    ChVector<> CqBr = Vcross(D2relB, pos2);

    Cx.Get_Cq_a()->ElementN(0) = CqAx.x();
    Cx.Get_Cq_a()->ElementN(1) = CqAx.y();
    Cx.Get_Cq_a()->ElementN(2) = CqAx.z();
    Cx.Get_Cq_a()->ElementN(3) = CqAr.x();
    Cx.Get_Cq_a()->ElementN(4) = CqAr.y();
    Cx.Get_Cq_a()->ElementN(5) = CqAr.z();

    Cx.Get_Cq_b()->ElementN(0) = CqBx.x();
    Cx.Get_Cq_b()->ElementN(1) = CqBx.y();
    Cx.Get_Cq_b()->ElementN(2) = CqBx.z();
    Cx.Get_Cq_b()->ElementN(3) = CqBr.x();
    Cx.Get_Cq_b()->ElementN(4) = CqBr.y();
    Cx.Get_Cq_b()->ElementN(5) = CqBr.z();

    //***TO DO***  C_dt? C_dtdt? (may be never used..)
}
Beispiel #4
0
vector wibblyrepulsor(vector v,vector r,float str,float dstr,float off,float fre)
{
  if (Vequal(v,r))
    return V(0,0);
  else {
    return Vmult((str+dstr*sin(off+frame*fre))/radius(Vsub(v,r)),Vnorm(Vsub(v,r)));
  }
}
Beispiel #5
0
vector forceat(vector v)
{
  vector vminusp,vminusq;
  
  if (Vequal(v,V(0,0)))
    return V(0,0);
  else
    return Vmult(0.001,Vadd(Vrotate(Vnorm(v),pi/2+pi/32+0*sin(frame*2*pi/10)),
           Vadd(inoutattractor(v,p,3,85),inoutattractor(v,q,2,381))));
}
Beispiel #6
0
vector massivebody(vector v)
{
  float tmp;
  
  tmp=radius(v);
  if (tmp>0.1 && tmp<100)
    return Vmult(strength/square(tmp),Vnorm(V(2*v.x*v.y,square(v.y)-square(v.x))));
  else
    return V(0,0);
}
Beispiel #7
0
ChCoordsys<> ChLinkDistance::GetLinkRelativeCoords() {
    ChVector<> D2local;
    ChVector<> D2temp = (Vnorm(Body1->TransformPointLocalToParent(pos1) - Body2->TransformPointLocalToParent(pos2)));
    ChVector<> D2rel = Body2->TransformDirectionParentToLocal(D2temp);
    ChVector<> Vx, Vy, Vz;
    ChMatrix33<> rel_matrix;
    XdirToDxDyDz(D2rel, VECT_Y, Vx, Vy, Vz);
    rel_matrix.Set_A_axis(Vx, Vy, Vz);

    Quaternion Ql2 = rel_matrix.Get_A_quaternion();
    return ChCoordsys<>(pos2, Ql2);
}
Beispiel #8
0
ChCoordsys<> ChLinkDistance::GetLinkRelativeCoords()
{
	ChVector<> D2local;
	ChVector<> D2temp=(Vnorm(Body1->Point_Body2World(&pos1)-Body2->Point_Body2World(&pos2)));
	ChVector<> D2rel = Body2->Dir_World2Body(&D2temp);
	ChVector<> Vx, Vy, Vz;
	ChVector<> Vsingul(VECT_Y);
	ChMatrix33<> rel_matrix;
	XdirToDxDyDz(&D2rel, &Vsingul, &Vx,  &Vy, &Vz);
	rel_matrix.Set_A_axis(Vx,Vy,Vz);

	Quaternion Ql2 = rel_matrix.Get_A_quaternion();
	return ChCoordsys<>(pos2, Ql2); 
}
Beispiel #9
0
void ChLinkSpring::UpdateForces(double mytime) {
    // Inherit force computation:
    // also base class can add its own forces.
    ChLinkMarkers::UpdateForces(mytime);

    spr_react = 0.0;
    Vector m_force;
    double deform = Get_SpringDeform();

    spr_react = spr_f * mod_f_time->Get_y(ChTime);
    spr_react -= (spr_k * mod_k_d->Get_y(deform) * mod_k_speed->Get_y(dist_dt)) * (deform);
    spr_react -= (spr_r * mod_r_d->Get_y(deform) * mod_r_speed->Get_y(dist_dt)) * (dist_dt);

    m_force = Vmul(Vnorm(relM.pos), spr_react);

    C_force = Vadd(C_force, m_force);
}
double ChCollisionUtils::PointLineDistance(Vector p, Vector dA, Vector dB, double& mu, int& is_insegment) {
    mu = -1.0;
    is_insegment = 0;
    double mdist = 10e34;

    Vector vseg = Vsub(dB, dA);
    Vector vdir = Vnorm(vseg);
    Vector vray = Vsub(p, dA);

    mdist = Vlength(Vcross(vray, vdir));
    mu = Vdot(vray, vdir) / Vlength(vseg);

    if ((mu >= 0) && (mu <= 1.0))
        is_insegment = 1;

    return mdist;
}
Beispiel #11
0
void Q_to_AngAxis(const ChQuaternion<double>& quat, double& angle, ChVector<double>& axis) {
    if (fabs(quat.e0()) < 0.99999999) {
        double arg = acos(quat.e0());
        double invsine = 1 / sin(arg);
        ChVector<double> vtemp;
        vtemp.x() = invsine * quat.e1();
        vtemp.y() = invsine * quat.e2();
        vtemp.z() = invsine * quat.e3();
        angle = 2 * arg;
        axis = Vnorm(vtemp);
    } else {
        axis.x() = 1;
        axis.y() = 0;
        axis.z() = 0;
        angle = 0;
    }
}
Beispiel #12
0
void Viewer::computeExtrusion(int nOrientation,
			      float *orientation,
			      int nScale,
			      float *scale,
			      int nCrossSection,
			      float *crossSection,
			      int nSpine,
			      float *spine,
			      float *c,   // OUT: coordinates
			      float *tc,  // OUT: texture coords
			      int *faces)     // OUT: face list
{
  int i, j, ci;

  // Xscp, Yscp, Zscp- columns of xform matrix to align cross section
  // with spine segments.
  float Xscp[3] = { 1.0, 0.0, 0.0};
  float Yscp[3] = { 0.0, 1.0, 0.0};
  float Zscp[3] = { 0.0, 0.0, 1.0};
  float lastZ[3];

  // Is the spine a closed curve (last pt == first pt)?
  bool spineClosed = (FPZERO(spine[ 3*(nSpine-1)+0 ] - spine[0]) &&
		      FPZERO(spine[ 3*(nSpine-1)+1 ] - spine[1]) &&
		      FPZERO(spine[ 3*(nSpine-1)+2 ] - spine[2]));
  
  // Is the spine a straight line?
  bool spineStraight = true;
  for (i = 1; i < nSpine-1; ++i)
    {
      float v1[3], v2[3];
      v1[0] = spine[3*(i-1)+0] - spine[3*(i)+0];
      v1[1] = spine[3*(i-1)+1] - spine[3*(i)+1];
      v1[2] = spine[3*(i-1)+2] - spine[3*(i)+2];
      v2[0] = spine[3*(i+1)+0] - spine[3*(i)+0];
      v2[1] = spine[3*(i+1)+1] - spine[3*(i)+1];
      v2[2] = spine[3*(i+1)+2] - spine[3*(i)+2];
      Vcross(v1, v2, v1);
      if (Vlength(v1) != 0.0)
	{
	  spineStraight = false;
	  Vnorm( v1 );
	  Vset( lastZ, v1 );
	  break;
	}
    }

  // If the spine is a straight line, compute a constant SCP xform
  if (spineStraight)
    {
      float V1[3] = { 0.0, 1.0, 0.0}, V2[3], V3[3];
      V2[0] = spine[3*(nSpine-1)+0] - spine[0];
      V2[1] = spine[3*(nSpine-1)+1] - spine[1];
      V2[2] = spine[3*(nSpine-1)+2] - spine[2];
      Vcross( V3, V2, V1 );
      float len = (float)Vlength(V3);
      if (len != 0.0)		// Not aligned with Y axis
	{
	  Vscale(V3, 1.0f/len);

	  float orient[4];		// Axis/angle
	  Vset(orient, V3);
	  orient[3] = acos(Vdot(V1,V2));
	  double scp[4][4];	        // xform matrix
	  Mrotation( scp, orient );
	  for (int k=0; k<3; ++k) {
	    Xscp[k] = (float)scp[0][k];
	    Yscp[k] = (float)scp[1][k];
	    Zscp[k] = (float)scp[2][k];
	  }
	}
    }

  // Orientation matrix
  double om[4][4];
  if (nOrientation == 1 && ! FPZERO(orientation[3]) )
    Mrotation( om, orientation );

  // Compute coordinates, texture coordinates:
  for (i = 0, ci = 0; i < nSpine; ++i, ci+=nCrossSection) {

    // Scale cross section
    for (j = 0; j < nCrossSection; ++j) {
      c[3*(ci+j)+0] = scale[0] * crossSection[ 2*j ];
      c[3*(ci+j)+1] = 0.0;
      c[3*(ci+j)+2] = scale[1] * crossSection[ 2*j+1 ];
    }

    // Compute Spine-aligned Cross-section Plane (SCP)
    if (! spineStraight)
      {
	float S1[3], S2[3];	// Spine vectors [i,i-1] and [i,i+1]
	int yi1, yi2, si1, s1i2, s2i2;

	if (spineClosed && (i == 0 || i == nSpine-1))
	  {
	    yi1 = 3*(nSpine-2);
	    yi2 = 3;
	    si1 = 0;
	    s1i2 = 3*(nSpine-2);
	    s2i2 = 3;
	  }
	else if (i == 0)
	  {
	    yi1 = 0;
	    yi2 = 3;
	    si1 = 3;
	    s1i2 = 0;
	    s2i2 = 6;
	  }
	else if (i == nSpine-1)
	  {
	    yi1 = 3*(nSpine-2);
	    yi2 = 3*(nSpine-1);
	    si1 = 3*(nSpine-2);
	    s1i2 = 3*(nSpine-3);
	    s2i2 = 3*(nSpine-1);
	  }
	else
	  {
	    yi1 = 3*(i-1);
	    yi2 = 3*(i+1);
	    si1 = 3*i;
	    s1i2 = 3*(i-1);
	    s2i2 = 3*(i+1);
	  }

	Vdiff( Yscp, &spine[yi2], &spine[yi1] );
	Vdiff( S1, &spine[s1i2], &spine[si1] );
	Vdiff( S2, &spine[s2i2], &spine[si1] );

	Vnorm( Yscp );
	Vset(lastZ, Zscp);	// Save last Zscp
	Vcross( Zscp, S2, S1 );

	float VlenZ = (float)Vlength(Zscp);
	if ( VlenZ == 0.0 )
	  Vset(Zscp, lastZ);
	else
	  Vscale( Zscp, 1.0f/VlenZ );

	if ((i > 0) && (Vdot( Zscp, lastZ ) < 0.0))
	  Vscale( Zscp, -1.0 );

	Vcross( Xscp, Yscp, Zscp );
      }

    // Rotate cross section into SCP
    for (j = 0; j < nCrossSection; ++j) {
      float cx, cy, cz;
      cx = c[3*(ci+j)+0]*Xscp[0]+c[3*(ci+j)+1]*Yscp[0]+c[3*(ci+j)+2]*Zscp[0];
      cy = c[3*(ci+j)+0]*Xscp[1]+c[3*(ci+j)+1]*Yscp[1]+c[3*(ci+j)+2]*Zscp[1];
      cz = c[3*(ci+j)+0]*Xscp[2]+c[3*(ci+j)+1]*Yscp[2]+c[3*(ci+j)+2]*Zscp[2];
      c[3*(ci+j)+0] = cx;
      c[3*(ci+j)+1] = cy;
      c[3*(ci+j)+2] = cz;
    }

    // Apply orientation
    if (! FPZERO(orientation[3]) )
      {
	if (nOrientation > 1)
	  Mrotation( om, orientation );

	for (j = 0; j < nCrossSection; ++j) {
	  float cx, cy, cz;
	  cx = (float)(c[3*(ci+j)+0]*om[0][0]+c[3*(ci+j)+1]*om[1][0]+c[3*(ci+j)+2]*om[2][0]);
	  cy = (float)(c[3*(ci+j)+0]*om[0][1]+c[3*(ci+j)+1]*om[1][1]+c[3*(ci+j)+2]*om[2][1]);
	  cz = (float)(c[3*(ci+j)+0]*om[0][2]+c[3*(ci+j)+1]*om[1][2]+c[3*(ci+j)+2]*om[2][2]);
	  c[3*(ci+j)+0] = cx;
	  c[3*(ci+j)+1] = cy;
	  c[3*(ci+j)+2] = cz;
	}
      }

    // Translate cross section
    for (j = 0; j < nCrossSection; ++j) {
      c[3*(ci+j)+0] += spine[3*i+0];
      c[3*(ci+j)+1] += spine[3*i+1];
      c[3*(ci+j)+2] += spine[3*i+2];

      // Texture coords
      tc[3*(ci+j)+0] = ((float) j) / (nCrossSection-1);
      tc[3*(ci+j)+1] = 1.0f - ((float) i) / (nSpine-1);
      tc[3*(ci+j)+2] = 0.0f;
    }

    if (nScale > 1) scale += 2;
    if (nOrientation > 1) orientation += 4;
  }

  // And compute face indices:
  if (faces)
    {
      int polyIndex = 0;
      for (i = 0, ci = 0; i < nSpine-1; ++i, ci+=nCrossSection) {
	for (j = 0; j < nCrossSection-1; ++j) {
	  faces[polyIndex + 0] = ci+j;
	  faces[polyIndex + 1] = ci+j+1;
	  faces[polyIndex + 2] = ci+j+1 + nCrossSection;
	  faces[polyIndex + 3] = ci+j + nCrossSection;
	  faces[polyIndex + 4] = -1;
	  polyIndex += 5;
	}
      }
    }
}
Beispiel #13
0
vector holder(vector v, float strength)
{
  return Vmult(strength*square(radius(v)),Vnorm(Vneg(v)));
}
Beispiel #14
0
void ChLinkPulley::UpdateTime (double mytime)
{
    // First, inherit to parent class
    ChLinkLock::UpdateTime(mytime);

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

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

	ChVector<> dcc_w = Vsub(Get_shaft_pos2(),
                            Get_shaft_pos1());

		// compute actual rotation of the two wheels (relative to truss).
    Vector md1 = abs_shaft1.GetA()->MatrT_x_Vect(dcc_w);
    md1.z = 0;  md1 = Vnorm (md1);
    Vector md2 = abs_shaft2.GetA()->MatrT_x_Vect(dcc_w);
    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;

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

        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);
		//***TODO***
    }


    // Move markers 1 and 2 to align them as pulley ends

	ChVector<> d21_w = dcc_w - Get_shaft_dir1()* Vdot (Get_shaft_dir1(), dcc_w);
	ChVector<> D21_w = Vnorm(d21_w);

	this->shaft_dist = d21_w.Length();
	
	ChVector<> U1_w = Vcross(Get_shaft_dir1(), D21_w);

	double gamma1 = acos( (r1-r2) / shaft_dist);

	ChVector<> Ru_w =  D21_w*cos(gamma1) + U1_w*sin(gamma1);
	ChVector<> Rl_w =  D21_w*cos(gamma1) - U1_w*sin(gamma1);

	this->belt_up1  = Get_shaft_pos1()+ Ru_w*r1;
	this->belt_low1 = Get_shaft_pos1()+ Rl_w*r1;
	this->belt_up2  = Get_shaft_pos1()+ d21_w + Ru_w*r2;
	this->belt_low2 = Get_shaft_pos1()+ d21_w + Rl_w*r2;

		// marker alignment
	ChMatrix33<> maU;
	ChMatrix33<> maL;

	ChVector<> Dxu = Vnorm(belt_up2 - belt_up1);
	ChVector<> Dyu = Ru_w;
	ChVector<> Dzu = Vnorm (Vcross(Dxu, Dyu));
	Dyu = Vnorm (Vcross(Dzu, Dxu));
	maU.Set_A_axis(Dxu,Dyu,Dzu);

            // ! 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);

	ChCoordsys<> newmarkpos;

        // move marker1 in proper positions
    newmarkpos.pos = this->belt_up1;
    newmarkpos.rot = maU.Get_A_quaternion();
    marker1->Impose_Abs_Coord(newmarkpos);        //move marker1 into teeth position
        // move marker2 in proper positions
    newmarkpos.pos = this->belt_up2;
    newmarkpos.rot = maU.Get_A_quaternion();
    marker2->Impose_Abs_Coord(newmarkpos);        //move marker2 into teeth position

	double phase_correction_up = m_delta*r1;
	double phase_correction_low = - phase_correction_up;
	double hU = Vlenght(belt_up2- belt_up1)  + phase_correction_up;
	double hL = Vlenght(belt_low2- belt_low1) + phase_correction_low;

        // imposed relative positions/speeds
    deltaC.pos = ChVector<>(-hU, 0, 0);
    deltaC_dt.pos = VNULL;
    deltaC_dtdt.pos = VNULL;

    deltaC.rot = QUNIT;             // no relative rotations imposed!
    deltaC_dt.rot = QNULL;
    deltaC_dtdt.rot = QNULL;
}
Beispiel #15
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;

}