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));
}
