float
Vangle (const vector_type in_vector,
const vector_type out_vector,
at_exception_type * exp)
{
vector_type v1 = normalize (in_vector);
vector_type v2 = normalize (out_vector);
return acos_d (Vdot (v2, v1), exp);
}
double MainWindow::singleSigma(double r)
{
double Gtot, E,dE, Emin,csh,sum,alpha,Ec,Uc,V;
V=Vbarrier(r);
Uc=Vdot();
double kT=this->T;
dE=0.1;
if(dE>=0.6) dE=0.5;
Ec=this->EFT;
// double g0=cohU(Ec, this->Ey, r1, V, Uc);
// double g0=sedlo(Ec, this->Ey, this->Ex, V);
if(kT==0)
{
// if(Ec>Uc) Gtot=cohU(Ec,this->Ey,r1,V,Uc);
if(Ec>Uc) Gtot=sedlo(Ec, this->Ey,this->Ex, V);
else Gtot=CUTOFF_SIGMA;
}
else
{ Gtot=0;
double GTunnel=0.;
double GOver=0.;
int G_type = this->typeCond;
Emin=Ec-40*kT;
double sumt=0.;
double aa=0.25*dE/kT;
if(Emin<Uc) Emin=Uc+dE;
for(E=Emin; E<=Ec+40*kT; E+=dE){
alpha=0.5*(E-Ec)/kT;
csh=1./cosh(alpha);
sum=aa*csh*csh;
sumt=sumt+sum;
// double g=cohU(E,this->Ey,r1,V,Uc);
double g=sedlo(E, this->Ey, this->Ex, V);
GTunnel+= this->gTun*sum;
GOver+= this->gOv*sum;
Gtot+=g*sum;
}
if(G_type==1) Gtot=GTunnel;
if(G_type==2) Gtot=GOver;
double eps=0.0075*this->U-1.02;
if(G_type==3) Gtot=GOver+GTunnel*exp(-eps/kT);
}
Gtot=Gtot*G_ser/(Gtot+G_ser);
if(Gtot<CUTOFF_SIGMA) return CUTOFF_SIGMA;
else
return Gtot;
}
// -----------------------------------------------------------------------------
// Utility function for characterizing the geometric contact between a disc with
// specified center location, normal direction, and radius and the terrain,
// assumed to be specified as a height field (over the x-y domain).
// This function returns false if no contact occurs. Otherwise, it sets the
// contact points on the disc (ptD) and on the terrain (ptT), the normal contact
// direction, and the resulting penetration depth (a positive value).
// -----------------------------------------------------------------------------
bool ChTire::disc_terrain_contact(const ChTerrain& terrain,
const ChVector<>& disc_center,
const ChVector<>& disc_normal,
double disc_radius,
ChCoordsys<>& contact,
double& depth) {
// Find terrain height below disc center. There is no contact if the disc
// center is below the terrain or farther away by more than its radius.
double hc = terrain.GetHeight(disc_center.x(), disc_center.y());
if (disc_center.z() <= hc || disc_center.z() >= hc + disc_radius)
return false;
// Find the lowest point on the disc. There is no contact if the disc is
// (almost) horizontal.
ChVector<> dir1 = Vcross(disc_normal, ChVector<>(0, 0, 1));
double sinTilt2 = dir1.Length2();
if (sinTilt2 < 1e-3)
return false;
// Contact point (lowest point on disc).
ChVector<> ptD = disc_center + disc_radius * Vcross(disc_normal, dir1 / sqrt(sinTilt2));
// Find terrain height at lowest point. No contact if lowest point is above
// the terrain.
double hp = terrain.GetHeight(ptD.x(), ptD.y());
if (ptD.z() > hp)
return false;
// Approximate the terrain with a plane. Define the projection of the lowest
// point onto this plane as the contact point on the terrain.
ChVector<> normal = terrain.GetNormal(ptD.x(), ptD.y());
ChVector<> longitudinal = Vcross(disc_normal, normal);
longitudinal.Normalize();
ChVector<> lateral = Vcross(normal, longitudinal);
ChMatrix33<> rot;
rot.Set_A_axis(longitudinal, lateral, normal);
contact.pos = ptD;
contact.rot = rot.Get_A_quaternion();
depth = Vdot(ChVector<>(0, 0, hp - ptD.z()), normal);
assert(depth > 0);
return true;
}
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;
}
double MainWindow::sedlo(double E, double Ey, double Ex, double V)
{ double alpha,G0,g,exp0,EE, Ep,Uc;
Uc=Vdot();
double a1=150;//80;//100;//500;//nm
double Va=V-12.50;//meV
V=V+2*exp(-this->T/0.6);
double a0=2/Ex*sqrt(E0*(V-Va));
double a2=a0/a1;
double U00=V-Va;
double U01=Va/(1-a2*a2);
this->gTun=0;
this->gOv=0;
G0=0;
EE=E-0.5*Ey-V;
Ep=E-0.5*Ey;
while(Ep>Uc)
{
if(Ep>Va)
{
alpha=-6.2832*EE/Ex;
exp0=exp(alpha);
g=1./(1+exp0);
}
else
{
double b0=sqrt(U00/(V-Ep));
double b1=a2*sqrt(U01/(U01-Ep));
double asinb0=asin(b0);
double asinb1=asin(b1);
double b2=1/(b0*b0);
double Z=-2*a0*sqrt(U00/E0)*(sqrt(b2-1)+asinb0*b2);
b2=1/(b1*b1);
Z=Z-2*a0*a2*sqrt(U01/E0)*((3.14159/2-asinb1)*b2-sqrt(b2-1));
g=exp(Z);
}
if(g<0.5) this->gTun+=g;
else this->gOv+=g;
G0+=g;
EE-=Ey;
Ep-=Ey;
}
return G0;
}
double ChSteeringController::Advance(const ChVehicle& vehicle, double step) {
// Calculate current "sentinel" location. This is a point at the look-ahead
// distance in front of the vehicle.
m_sentinel = vehicle.GetChassisBody()->GetFrame_REF_to_abs().TransformPointLocalToParent(ChVector<>(m_dist, 0, 0));
// Calculate current "target" location.
CalcTargetLocation();
// If data collection is enabled, append current target and sentinel locations.
if (m_collect) {
*m_csv << vehicle.GetChTime() << m_target << m_sentinel << std::endl;
}
// The "error" vector is the projection onto the horizontal plane (z=0) of
// the vector between sentinel and target.
ChVector<> err_vec = m_target - m_sentinel;
err_vec.z() = 0;
// Calculate the sign of the angle between the projections of the sentinel
// vector and the target vector (with origin at vehicle location).
ChVector<> sentinel_vec = m_sentinel - vehicle.GetVehiclePos();
sentinel_vec.z() = 0;
ChVector<> target_vec = m_target - vehicle.GetVehiclePos();
target_vec.z() = 0;
double temp = Vdot(Vcross(sentinel_vec, target_vec), ChVector<>(0, 0, 1));
// Calculate current error (magnitude).
double err = ChSignum(temp) * err_vec.Length();
// Estimate error derivative (backward FD approximation).
m_errd = (err - m_err) / step;
// Calculate current error integral (trapezoidal rule).
m_erri += (err + m_err) * step / 2;
// Cache new error
m_err = err;
// Return PID output (steering value)
return m_Kp * m_err + m_Ki * m_erri + m_Kd * m_errd;
}
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;
}
}
}
}
double ChPathSteeringControllerXT::Advance(const ChVehicle& vehicle, double step) {
// Calculate current "sentinel" location. This is a point at the look-ahead
// distance in front of the vehicle.
m_sentinel = vehicle.GetChassisBody()->GetFrame_REF_to_abs().TransformPointLocalToParent(ChVector<>(m_dist, 0, 0));
m_vel = vehicle.GetVehiclePointVelocity(ChVector<>(0,0,0));
if(!m_filters_initialized) {
// first time we know about step size
m_HeadErrDelay.Config(step, m_T1_delay);
m_AckermannAngleDelay.Config(step, m_T1_delay);
m_PathErrCtl.Config(step,0.3,0.15,m_Kp);
m_filters_initialized = true;
}
// Calculate current "target" location.
CalcTargetLocation();
// If data collection is enabled, append current target and sentinel locations.
if (m_collect) {
*m_csv << vehicle.GetChTime() << m_target << m_sentinel << std::endl;
}
// The "error" vector is the projection onto the horizontal plane (z=0) of
// the vector between sentinel and target.
ChVector<> err_vec = m_target - m_sentinel;
err_vec.z() = 0;
// Calculate the sign of the angle between the projections of the sentinel
// vector and the target vector (with origin at vehicle location).
ChVector<> sentinel_vec = m_sentinel - vehicle.GetVehiclePos();
sentinel_vec.z() = 0;
ChVector<> target_vec = m_target - vehicle.GetVehiclePos();
target_vec.z() = 0;
double temp = Vdot(Vcross(sentinel_vec, target_vec), ChVector<>(0, 0, 1));
// Calculate current lateral error.
double y_err = ChSignum(temp) * err_vec.Length();
double y_err_out = m_PathErrCtl.Filter(y_err);
// Calculate the heading error
ChVector<> veh_head = vehicle.GetVehicleRot().GetXaxis();
ChVector<> path_head = m_ptangent;
double h_err = CalcHeadingError(veh_head,path_head);
double h_err_out = m_HeadErrDelay.Filter(h_err);
// Calculate the Ackermann angle
double a_err = CalcAckermannAngle();
double a_err_out = m_AckermannAngleDelay.Filter(a_err);
// Calculate the resultant steering signal
double res = m_Wy * y_err_out + m_Wh * h_err_out + m_Wa * a_err_out;
// Additional processing is necessary: counter steer constraint
// in left bending curves only left steering allowed
// in right bending curves only right steering allowed
// |res| is never allowed to grow above 1
ChVector<> veh_left = vehicle.GetVehicleRot().GetYaxis();
ChVector<> path_left = m_pnormal;
int crvcode = CalcCurvatureCode(veh_left,path_left);
switch(crvcode) {
case 1:
m_res = ChClamp<>(res,0.0,1.0);
break;
case -1:
m_res = ChClamp<>(res,-1.0,0.0);
break;
default:
case 0:
m_res = ChClamp<>(res,-1.0,1.0);
break;
}
return m_res;
}
//!!!!!!
void MainWindow::computeEF_TU()
{
double E,EFT0,EFT1,EFT2 ;
double dE=0.1;
double sum, sum1, Area, sum10, sum11, sum12;
double Ucur=this->U;//!!!!!!!!!!!
this->U=Vg0;
double Vd0=Vdot();
this->U=Ucur;
double aa=(sqrt(1250.)-350)/Delta_r;
aa=aa*aa;
aa=4*this->U/(1+aa);
// aa=0;
if(this->T==0)
{
EFT1=aa+EF0+Vdot()-Vd0+Cg0*(Ucur-Vg0);//!!!!!!!!!!
}
else
{
//T!=0
double Vd=Vdot();
this->EF=aa+EF0+Vd-Vd0+Cg0*(Ucur-Vg0);
int NE=int((this->EF+40*this->T-Vd)/dE);
this->AreaEf.resize(NE,0.0);
sum=0;
EFT1=this->EF-1;//!!!!!!!!!!!!!!!!!!!!!!!!
for (int i=0; i< NE; ++i)
{
E=dE*(i+1)+Vd;
Area=AreaE(E)/10000;
this->AreaEf[i]=Area;
if(E<=this->EF) sum=sum+Area;
}
// this->density=sum;
if(sum!=0)
{
EFT0=EFT1;//this->EF-1.;//!!!!!!!!!!!!!!!!!
sum1=computeSum(NE, dE, Vd, EFT0);
while(sum1>sum)
{
EFT0=EFT0-1;
sum1=computeSum(NE, dE, Vd, EFT0);
}
sum10=sum1;
EFT1=EFT0+1;
sum11=computeSum(NE, dE, Vd, EFT1);
// int j=0;
while(fabs(sum11-sum)>0.001*sum)
{
EFT2=EFT1-(sum11-sum)*(EFT1-EFT0)/(sum11-sum10);
sum12=computeSum(NE, dE, Vd, EFT2);
// j++;
if(sum12>sum&&sum11<sum||sum12<sum&& sum11>sum)
{
sum10=sum11;
EFT0=EFT1;
}
sum11=sum12;
EFT1=EFT2;
}
}
}
this->EFT=EFT1;
}
void MainWindow::computeEFU()
{
double E,EFT0,EFT1,EFT2 ;
double dE=0.1;
double sum, sum1, Area, sum10, sum11, sum12;
int NU=1+int( (this->Umax-this->Umin)/this->dU );
this->EFUarray.resize(NU,0.0);
// double Ucur=this->U;
this->U=Vg0;
double Vd0=Vdot();
double aa1=(sqrt(1250.)-350)/Delta_r;
aa1=aa1*aa1;
aa1=4/(1+aa1);
// double EF00this->EF0;
if(this->T==0) {
int j=0;
for(double x=this->Umin; x<=this->Umax; x+=this->dU)
{ this->U=x;
double aa=aa1*this->U;
// aa=0;
EFT1=aa+EF0+Vdot()-Vd0+Cg0*(this->U-Vg0);
if(j<this->EFUarray.size())
{
this->EFUarray[j]=EFT1;
j++;
this->EFT=EFT1;
}
}
return;
}
int j=0;
for(double x=this->Umin; x<=this->Umax; x+=this->dU)
{
this->U=x;
double Vd=Vdot();
double aa=aa1*this->U;
this->EF=aa+EF0+Vd-Vd0+Cg0*(this->U-Vg0);
int NE = int( (this->EF+40*this->T-Vd)/dE );
this->AreaEf.resize(NE,0.0);
sum=0;
EFT1=this->EF-1;
for (int i=0; i< NE; ++i)
{
E=dE*(i+1)+Vd;
Area=AreaE(E)/10000;
this->AreaEf[i]=Area;
if(E<=this->EF) sum=sum+Area;
}
if(sum==0)
{
j++;
}
else
{
EFT0=EFT1;
sum1=computeSum(NE, dE, Vd, EFT0);
while(sum1>sum)
{
EFT0=EFT0-1;
sum1=computeSum(NE, dE, Vd, EFT0);
}
sum10=sum1;
EFT1=EFT0+1;
sum11=computeSum(NE, dE, Vd, EFT1);
while(fabs(sum11-sum)>0.001*sum)
{
EFT2=EFT1-(sum11-sum)*(EFT1-EFT0)/(sum11-sum10);
sum12=computeSum(NE, dE, Vd, EFT2);
if(sum12>sum&&sum11<sum||sum12<sum&& sum11>sum)
{
sum10=sum11;
EFT0=EFT1;
}
sum11=sum12;
EFT1=EFT2;
}
if(j<this->EFUarray.size())
{
this->EFUarray[j]=EFT1;
j++;
this->EFT=EFT1;
}
else break;
}
}
}
//!!!!!!!
void MainWindow::computeEFT()
{
double E,EFT0,EFT1,EFT2 ;
int NT=1+int( (this->Tmax-this->Tmin)/this->dT );
this->EFTarray.resize(NT,0.0);
// printf("EFTarray has %i points",NT);
/* for(int j=0; j<NT; j++)
{
this->EFTarray[j]=EF0;
}
*/
double dE=0.1;
double Ucur=this->U;
this->U=Vg0;
double Vd0=Vdot();
this->U=Ucur;
double Vd=Vdot();
double aa1=(sqrt(1250.)-350)/Delta_r;
aa1=aa1*aa1;
aa1=4/(1+aa1);
double aa=aa1*this->U;
// double EF00=this->EF0;
this->EF=aa+EF0+Vd-Vd0+Cg0*(this->U-Vg0);
// return;
int NE = 1+int( (this->EF+40*this->Tmax-Vdot())/dE );
if(NE<0) return;
printf("AreaEf has %i points",NE);
this->AreaEf.resize(NE,0.0);
double sum, sum1, Area, sum10, sum11, sum12;
sum=0;
for (int i=0; i< NE; ++i)
{
E=dE*(i+1)+Vd;
Area=AreaE(E)/10000;
this->AreaEf[i]=Area;
if(E<=this->EF) sum=sum+Area;
}
if(sum==0) return;
else
{
EFT1=this->EF-1.;
for(int j=0; j<NT; j++)
{
this->T=this->Tmax-this->dT*j;
EFT0=EFT1;
sum1=computeSum(NE, dE, Vd, EFT0);
while(sum1>sum)
{
EFT0=EFT0-1;
sum1=computeSum(NE, dE, Vd, EFT0);
}
sum10=sum1;
EFT1=EFT0+1;
sum11=computeSum(NE, dE, Vd, EFT1);
while(fabs(sum11-sum)>0.001*sum)
{
EFT2=EFT1-(sum11-sum)*(EFT1-EFT0)/(sum11-sum10);
sum12=computeSum(NE, dE, Vd, EFT2);
if(sum12>sum&&sum11<sum||sum12<sum&& sum11>sum)
{
sum10=sum11;
EFT0=EFT1;
}
sum11=sum12;
EFT1=EFT2;
}
this->EFTarray[j]=EFT1;
}
}
}
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;
}
int
NewmarkSensitivityIntegrator::formEleResidual(FE_Element *theEle)
{
if (sensitivityFlag == 0) { // NO SENSITIVITY ANALYSIS
this->Newmark::formEleResidual(theEle);
}
else { // (ASSEMBLE ALL TERMS)
theEle->zeroResidual();
// Compute the time-stepping parameters on the form
// udotdot = a1*ui+1 + a2*ui + a3*udoti + a4*udotdoti
// udot = a5*ui+1 + a6*ui + a7*udoti + a8*udotdoti
// (see p. 166 of Chopra)
// The constants are:
// a1 = 1.0/(beta*dt*dt)
// a2 = -1.0/(beta*dt*dt)
// a3 = -1.0/beta*dt
// a4 = 1.0 - 1.0/(2.0*beta)
// a5 = gamma/(beta*dt)
// a6 = -gamma/(beta*dt)
// a7 = 1.0 - gamma/beta
// a8 = 1.0 - gamma/(2.0*beta)
// We can make use of the data members c2 and c3 of this class.
// As long as disp==true, they are defined as:
// c2 = gamma/(beta*dt)
// c3 = 1.0/(beta*dt*dt)
// So, the constants can be computed as follows:
if (displ==false) {
opserr << "ERROR: Newmark::formEleResidual() -- the implemented"
<< " scheme only works if the displ variable is set to true." << endln;
}
double a2 = -c3;
double a3 = -c2/gamma;
double a4 = 1.0 - 1.0/(2.0*beta);
double a6 = -c2;
double a7 = 1.0 - gamma/beta;
double dt = gamma/(beta*c2);
double a8 = dt*(1.0 - gamma/(2.0*beta));
// Obtain sensitivity vectors from previous step
int vectorSize = U->Size();
Vector V(vectorSize);
Vector Vdot(vectorSize);
Vector Vdotdot(vectorSize);
int i, loc;
AnalysisModel *myModel = this->getAnalysisModel();
DOF_GrpIter &theDOFs = myModel->getDOFs();
DOF_Group *dofPtr;
while ((dofPtr = theDOFs()) != 0) {
const ID &id = dofPtr->getID();
int idSize = id.Size();
const Vector &dispSens = dofPtr->getDispSensitivity(gradNumber);
for (i=0; i < idSize; i++) {
loc = id(i);
if (loc >= 0) {
V(loc) = dispSens(i);
}
}
const Vector &velSens = dofPtr->getVelSensitivity(gradNumber);
for (i=0; i < idSize; i++) {
loc = id(i);
if (loc >= 0) {
Vdot(loc) = velSens(i);
}
}
const Vector &accelSens = dofPtr->getAccSensitivity(gradNumber);
for (i=0; i < idSize; i++) {
loc = id(i);
if (loc >= 0) {
Vdotdot(loc) = accelSens(i);
}
}
}
// Pre-compute the vectors involving a2, a3, etc.
//Vector tmp1 = V*a2 + Vdot*a3 + Vdotdot*a4;
Vector tmp1(vectorSize);
tmp1.addVector(0.0, V, a2);
tmp1.addVector(1.0, Vdot, a3);
tmp1.addVector(1.0, Vdotdot, a4);
//Vector tmp2 = V*a6 + Vdot*a7 + Vdotdot*a8;
Vector tmp2(vectorSize);
tmp2.addVector(0.0, V, a6);
tmp2.addVector(1.0, Vdot, a7);
tmp2.addVector(1.0, Vdotdot, a8);
if (massMatrixMultiplicator == 0)
massMatrixMultiplicator = new Vector(tmp1.Size());
if (dampingMatrixMultiplicator == 0)
dampingMatrixMultiplicator = new Vector(tmp2.Size());
(*massMatrixMultiplicator) = tmp1;
(*dampingMatrixMultiplicator) = tmp2;
// Now we're ready to make calls to the FE Element:
// The term -dPint/dh|u fixed
theEle->addResistingForceSensitivity(gradNumber);
// The term -dM/dh*acc
theEle->addM_ForceSensitivity(gradNumber, *Udotdot, -1.0);
// The term -M*(a2*v + a3*vdot + a4*vdotdot)
theEle->addM_Force(*massMatrixMultiplicator,-1.0);
// The term -C*(a6*v + a7*vdot + a8*vdotdot)
theEle->addD_Force(*dampingMatrixMultiplicator,-1.0);
// The term -dC/dh*vel
theEle->addD_ForceSensitivity(gradNumber, *Udot,-1.0);
}
return 0;
}
int
NewmarkSensitivityIntegrator::saveSensitivity(const Vector & vNew,int gradNum,int numGrads)
{
// Compute Newmark parameters in general notation
double a1 = c3;
double a2 = -c3;
double a3 = -c2/gamma;
double a4 = 1.0 - 1.0/(2.0*beta);
double a5 = c2;
double a6 = -c2;
double a7 = 1.0 - gamma/beta;
double dt = gamma/(beta*c2);
double a8 = dt*(1.0 - gamma/(2.0*beta));
// Recover sensitivity results from previous step
int vectorSize = U->Size();
Vector V(vectorSize);
Vector Vdot(vectorSize);
Vector Vdotdot(vectorSize);
int i, loc;
AnalysisModel *myModel = this->getAnalysisModel();
DOF_GrpIter &theDOFs = myModel->getDOFs();
DOF_Group *dofPtr;
while ((dofPtr = theDOFs()) != 0) {
const ID &id = dofPtr->getID();
int idSize = id.Size();
const Vector &dispSens = dofPtr->getDispSensitivity(gradNumber);
for (i=0; i < idSize; i++) {
loc = id(i);
if (loc >= 0) {
V(loc) = dispSens(i);
}
}
const Vector &velSens = dofPtr->getVelSensitivity(gradNumber);
for (i=0; i < idSize; i++) {
loc = id(i);
if (loc >= 0) {
Vdot(loc) = velSens(i);
}
}
const Vector &accelSens = dofPtr->getAccSensitivity(gradNumber);
for (i=0; i < idSize; i++) {
loc = id(i);
if (loc >= 0) {
Vdotdot(loc) = accelSens(i);
}
}
}
// Compute new acceleration and velocity vectors:
Vector vdotNew(vectorSize);
Vector vdotdotNew(vectorSize);
//(*vdotdotNewPtr) = vNew*a1 + V*a2 + Vdot*a3 + Vdotdot*a4;
vdotdotNew.addVector(0.0, vNew, a1);
vdotdotNew.addVector(1.0, V, a2);
vdotdotNew.addVector(1.0, Vdot, a3);
vdotdotNew.addVector(1.0, Vdotdot, a4);
//(*vdotNewPtr) = vNew*a5 + V*a6 + Vdot*a7 + Vdotdot*a8;
vdotNew.addVector(0.0, vNew, a5);
vdotNew.addVector(1.0, V, a6);
vdotNew.addVector(1.0, Vdot, a7);
vdotNew.addVector(1.0, Vdotdot, a8);
// Now we can save vNew, vdotNew and vdotdotNew
DOF_GrpIter &theDOFGrps = myModel->getDOFs();
DOF_Group *dofPtr1;
while ( (dofPtr1 = theDOFGrps() ) != 0) {
dofPtr1->saveSensitivity(vNew,vdotNew,vdotdotNew,gradNum,numGrads);
}
return 0;
}
void ChLinkBrake::UpdateForces (double mytime)
{
// First, inherit to parent class
ChLinkLock::UpdateForces(mytime);
if (this->IsDisabled()) return;
// then, if not sticking,
if (this->brake_torque)
{
if (brake_mode == BRAKE_ROTATION)
{
if ( ((ChLinkMaskLF*)mask)->Constr_E3().IsActive() == false)
{
int mdir;
Vector mv_torque = Vmul(VECT_Z, this->brake_torque);
mdir = 0; // clockwise torque
if (Vdot(this->relWvel, mv_torque) > 0.0)
{
mv_torque = Vmul (mv_torque, -1.0); // keep torque always opposed to ang speed.
mdir = 1; // counterclockwise torque
}
if (mdir != this->last_dir)
this->must_stick = TRUE;
this->last_dir = mdir;
// +++ADD TO LINK TORQUE VECTOR
C_torque = Vadd(C_torque, mv_torque);
}
}
if (brake_mode == BRAKE_TRANSLATEX)
{
if ( ((ChLinkMaskLF*)mask)->Constr_X().IsActive() == false)
{
int mdir;
Vector mv_force = Vmul(VECT_X, this->brake_torque);
mdir = 0; // F--> rear motion: frontfacing break force
if (this->relM_dt.pos.x > 0.0)
{
mv_force = Vmul (mv_force, -1.0); // break force always opposed to speed
mdir = 1; // F<-- backfacing breakforce for front motion
}
if (mdir != this->last_dir)
this->must_stick = TRUE;
this->last_dir = mdir;
// +++ADD TO LINK TORQUE VECTOR
C_force = Vadd(C_force, mv_force);
}
}
}
// turn off sticking feature if stick ration not > 1.0
if (this->stick_ratio <= 1.0)
must_stick = FALSE;
}
