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))); }
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))); } }
/* Isolation is a measure of loneliness of a repulsor */ float isolation(rep r, int c) { int i; float rmin; rmin=2.0; for (i=0; i<c; i++) { if (radius(Vsub(r.pos,repulsor[i].pos))<rmin) rmin=radius(Vsub(r.pos,repulsor[i].pos)); } return rmin; }
int Vequal(vector u,vector v) { if (radius(Vsub(u,v))<0.001) return 1; else return 0; }
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; }
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; }
ChNarrowPhaseCollider::eCollSuccess CHOBBcollider::ComputeCollisions( ChMatrix33<>& R1, Vector T1, ChCollisionTree *oc1, ChMatrix33<>& R2, Vector T2, ChCollisionTree *oc2, eCollMode flag) { double t1 = GetTime(); // INHERIT parent class behaviour if (ChNarrowPhaseCollider::ComputeCollisions( R1, T1, oc1, R2, T2, oc2, flag) != ChC_RESULT_OK) return ChC_RESULT_GENERICERROR; // Downcasting CHOBBTree* o1 = (CHOBBTree*)oc1; CHOBBTree* o2 = (CHOBBTree*)oc2; // clear the stats this->num_bv_tests = 0; this->num_geo_tests = 0; // compute the transform from o1->child(0) to o2->child(0) static ChMatrix33<> Rtemp; static ChMatrix33<> bR; static Vector bT; static Vector Ttemp; Rtemp.MatrMultiply(this->R,o2->child(0)->Rot); // MxM(Rtemp,this->R,o2->child(0)->R); bR.MatrMultiply(o1->child(0)->Rot, Rtemp); // MTxM(R,o1->child(0)->R,Rtemp); Ttemp = ChTrasform<>::TrasformLocalToParent(o2->child(0)->To, this->T, this->R); // MxVpV(Ttemp,this->R,o2->child(0)->To,this->T); Ttemp = Vsub(Ttemp, o1->child(0)->To); // VmV(Ttemp,Ttemp,o1->child(0)->To); bT = o1->child(0)->Rot.MatrT_x_Vect(Ttemp); // MTxV(T,o1->child(0)->R,Ttemp); // now start with both top level BVs CollideRecurse(bR,bT,o1,0,o2,0,flag); double t2 = GetTime(); this->query_time_secs = t2 - t1; return ChC_RESULT_OK; }
void ChBodyFrame::To_abs_forcetorque(const ChVector<>& force, const ChVector<>& appl_point, bool local, ChVector<>& resultforce, ChVector<>& resulttorque) { if (local) { // local space ChVector<> mforce_abs = TransformDirectionLocalToParent(force); resultforce = mforce_abs; resulttorque = Vcross(TransformDirectionLocalToParent(appl_point), mforce_abs); } else { // absolute space resultforce = force; resulttorque = Vcross(Vsub(appl_point, coord.pos), force); } }
ChNarrowPhaseCollider::eCollSuccess ChNarrowPhaseCollider::ComputeCollisions(ChMatrix33<>& aR1, Vector aT1, ChCollisionTree* o1, ChMatrix33<>& aR2, Vector aT2, ChCollisionTree* o2, eCollMode flag) { // collision models must be here if (!o1 || !o2) return ChC_RESULT_GENERICERROR; // make sure that the models are built if (o1->build_state != ChCollisionTree::ChC_BUILD_STATE_PROCESSED) return ChC_RESULT_MODELSNOTBUILT; if (o2->build_state != ChCollisionTree::ChC_BUILD_STATE_PROCESSED) return ChC_RESULT_MODELSNOTBUILT; // clear the stats this->num_bv_tests = 0; this->num_geo_tests = 0; // don't release the memory, // DONT reset the num_collision_pairs counter (may be multiple calls for different object couples) // this->ClearPairsList(); // Precompute useful matrices this->R.MatrTMultiply(aR1, aR2); // MTxM(this->R,R1,R2); static Vector Ttemp; Ttemp = Vsub(aT2, aT1); // VmV(Ttemp, T2, T1); this->T = aR1.MatrT_x_Vect(Ttemp); // MTxV(this->T, R1, Ttemp); this->T1 = aT1; this->T2 = aT2; this->R1.CopyFromMatrix(aR1); this->R2.CopyFromMatrix(aR2); // // CHILD CLASSES SHOULD IMPLEMENT THE REST.... // .....(ex. code for collisions between AABB and AABB models, etc. // return ChC_RESULT_OK; }
ChQuaternion<double> AngleDTDT_to_QuatDTDT(AngleSet angset, const ChVector<double>& mangles, const ChQuaternion<double>& q) { ChQuaternion<double> res; ChQuaternion<double> qa, qb; ChVector<double> ang0, angA, angB; double hsquared = CH_LOWTOL; ang0 = Quat_to_Angle(angset, q); angA = Vsub(ang0, Vmul(mangles, hsquared)); angB = Vadd(ang0, Vmul(mangles, hsquared)); qa = Angle_to_Quat(angset, angA); qb = Angle_to_Quat(angset, angB); res = Qscale(Qadd(Qadd(qa, qb), Qscale(q, -2)), 1 / hsquared); return res; }
void CHAABBcollider::CollideRecurse( CHAABBTree *o1, int b1, CHAABBTree *o2, int b2, eCollMode flag) { CHAABB* box1 = o1->child(b1); CHAABB* box2 = o2->child(b2); this->num_bv_tests++; // first thing, see if we're overlapping static Vector Translation; Translation = Vsub (this->T, box1->To); Translation = Vadd (Translation, this->R.Matr_x_Vect(box2->To)); if (!CHAABB::AABB_Overlap(this->R, this->Rabs, Translation, box1, box2)) return; // if we are, see if we test triangles next int l1 = box1->IsLeaf(); int l2 = box2->IsLeaf(); // // CASE TWO LEAVES // if (l1 && l2) { this->num_geo_tests++; // transform the points in b2 into space of b1, then compare ChGeometry* mgeo1 = o1->geometries[box1->GetGeometryIndex()]; ChGeometry* mgeo2 = o2->geometries[box2->GetGeometryIndex()]; bool just_intersect = false; if (flag==ChNarrowPhaseCollider::ChC_FIRST_CONTACT) just_intersect = true; ChGeometryCollider::ComputeCollisions(*mgeo1, &this->R1, &this->T1, *mgeo2, &this->R2, &this->T2, *this, &this->R, &this->T, just_intersect); return; } // we dont, so decide whose children to visit next double sz1 = box1->GetSize(); double sz2 = box2->GetSize(); if (l2 || (!l1 && (sz1 > sz2))) { int c1 = box1->GetFirstChildIndex(); int c2 = box1->GetSecondChildIndex(); CollideRecurse(o1,c1,o2,b2,flag); if ((flag == ChC_FIRST_CONTACT) && (this->GetNumPairs() > 0)) return; CollideRecurse(o1,c2,o2,b2,flag); } else { int c1 = box2->GetFirstChildIndex(); int c2 = box2->GetSecondChildIndex(); CollideRecurse(o1,b1,o2,c1,flag); if ((flag == ChC_FIRST_CONTACT) && (this->GetNumPairs() > 0)) return; CollideRecurse(o1,b1,o2,c2,flag); } }
vector MBmove(vector v, vector m) { return massivebody(Vsub(v,m)); }
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; }
double vdiff2(xyz_t v, xyz_t u) { return Vmag2(Vsub(v, u)); }