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..) }
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))); }
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..) }
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))); } }
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)))); }
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); }
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); }
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); }
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; }
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; } }
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; } } } }
vector holder(vector v, float strength) { return Vmult(strength*square(radius(v)),Vnorm(Vneg(v))); }
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; }
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; }