inline void MatrixAngles(const matrix3x4 &matrix, Angle &angles)
{
Vector forward, left, up;
forward[0] = matrix[0][0];
forward[1] = matrix[1][0];
forward[2] = matrix[2][0];
left[0] = matrix[0][1];
left[1] = matrix[1][1];
left[2] = matrix[2][1];
up[2] = matrix[2][2];
float len2d = forward.Length2D();
if (len2d > 0.001f)
{
angles.x = Rad2Deg(Atan(-forward.z, len2d));
angles.y = Rad2Deg(Atan(forward.y, forward.x));
angles.z = Rad2Deg(Atan(left.z, up.z));
}
else
{
angles.x = Rad2Deg(Atan(-forward.z, len2d));
angles.y = Rad2Deg(Atan(-left.x, left.y));
angles.z = 0.f;
}
}
void Kick(Environment *env , int robot , Vector3D ToPos )
{
Mydata * p;
p=(Mydata *)env->userData;
Vector3D ball = p->curball; //这个球的位置不保险!!!
Vector3D RobotToBall; //人和球的相对位置
RobotToBall.x = ball.x - p->robot[robot].pos.x ;
RobotToBall.y = ball.y - p->robot[robot].pos.y ;
RobotToBall.z = Atan(p->robot[robot].pos , ball);
Vector3D BallToGate ; //球和球门的相对位置
BallToGate.x = ToPos.x - ball.x ;
BallToGate.y = ToPos.y - ball.y ;
BallToGate.z = Atan(ball , ToPos);
double RunAngle ;
RunAngle = RobotToBall.z - BallToGate.z;
ReAngle(RunAngle);
RunAngle = RobotToBall.z + RunAngle / 2 ; // 可以调整 2
ReAngle(RunAngle);
PAngle(env,robot,RunAngle,125);
}
double Atan(Vector3D begin,Vector3D end)
{
double y,x;
y=end.y - begin.y ;
x=end.x - begin.x ;
return Atan(y,x);
}
inline void
MakeDiscreteFourier( DistMatrix<Complex<R>,U,V>& A )
{
#ifndef RELEASE
CallStackEntry entry("MakeDiscreteFourier");
#endif
typedef Complex<R> F;
const int m = A.Height();
const int n = A.Width();
if( m != n )
throw std::logic_error("Cannot make a non-square DFT matrix");
const R pi = 4*Atan( R(1) );
const F nSqrt = Sqrt( R(n) );
const int localHeight = A.LocalHeight();
const int localWidth = A.LocalWidth();
const int colShift = A.ColShift();
const int rowShift = A.RowShift();
const int colStride = A.ColStride();
const int rowStride = A.RowStride();
for( int jLocal=0; jLocal<localWidth; ++jLocal )
{
const int j = rowShift + jLocal*rowStride;
for( int iLocal=0; iLocal<localHeight; ++iLocal )
{
const int i = colShift + iLocal*colStride;
A.SetLocal( iLocal, jLocal, Exp(-2*pi*i*j/n)/nSqrt );
const R theta = -2*pi*i*j/n;
const Complex<R> alpha( Cos(theta), Sin(theta) );
A.SetLocal( iLocal, jLocal, alpha/nSqrt );
}
}
}
int timeneed(Environment*env,int i,Vector3D go)
{
Mydata * p;
p=(Mydata *)env->userData;
double an;
double s;
double dis=0;
int t=0;
double vo;
double to;
an=Atan(p->robot[i].pos,go);
s=Distance(p->robot[i].pos,go);
t=int(s/1.9);
vo=p->myspeed[i].x*Cos(an)+p->myspeed[i].y*Cos(an-90);
to= log(1-vo/1.88)/-0.066;
while(1)
{
if(t>=35)
break;
if(s-dis<3)break;
dis=1.88*t+1.88 /0.066*(exp(-0.066*(to+t))-exp(-0.066*to));
t++;
}
return(t);
}
double Atan(double x)
{
if ((x>=1) || (x<=1))
return (x/(1+0.28*x*x));
else
return (PI/2 - Atan(1/x));
}
float Atan(float x)
{
if ((x >= 1) || (x <= 1))
return (x / (1 + 0.28 * x * x));
else
return (PI / 2 - Atan(1 / x));
}
void UniformHelmholtzGreens
( ElementalMatrix<Complex<Real>>& A, Int n, Real lambda )
{
EL_DEBUG_CSE
typedef Complex<Real> C;
const Real pi = 4*Atan( Real(1) );
const Real k0 = 2*pi/lambda;
const Grid& g = A.Grid();
// Generate a list of n uniform samples from the 3D unit ball
DistMatrix<Real,STAR,VR> X_STAR_VR(3,n,g);
for( Int jLoc=0; jLoc<X_STAR_VR.LocalWidth(); ++jLoc )
{
Real x0, x1, x2;
// Sample uniformly from [-1,+1]^3 until a point is drawn from the ball
while( true )
{
x0 = SampleUniform( Real(-1), Real(1) );
x1 = SampleUniform( Real(-1), Real(1) );
x2 = SampleUniform( Real(-1), Real(1) );
const Real radiusSq = x0*x0 + x1*x1 + x2*x2;
if( radiusSq > 0 && radiusSq <= 1 )
break;
}
X_STAR_VR.SetLocal( 0, jLoc, x0 );
X_STAR_VR.SetLocal( 1, jLoc, x1 );
X_STAR_VR.SetLocal( 2, jLoc, x2 );
}
DistMatrix<Real,STAR,STAR> X_STAR_STAR( X_STAR_VR );
A.Resize( n, n );
for( Int jLoc=0; jLoc<A.LocalWidth(); ++jLoc )
{
const Int j = A.GlobalCol(jLoc);
const Real xj0 = X_STAR_STAR.GetLocal(0,j);
const Real xj1 = X_STAR_STAR.GetLocal(1,j);
const Real xj2 = X_STAR_STAR.GetLocal(2,j);
for( Int iLoc=0; iLoc<A.LocalHeight(); ++iLoc )
{
const Int i = A.GlobalRow(iLoc);
if( i == j )
{
A.SetLocal( iLoc, jLoc, 0 );
}
else
{
const Real d0 = X_STAR_STAR.GetLocal(0,i)-xj0;
const Real d1 = X_STAR_STAR.GetLocal(1,i)-xj1;
const Real d2 = X_STAR_STAR.GetLocal(2,i)-xj2;
const Real gamma = k0*Sqrt(d0*d0+d1*d1+d2*d2);
const Real realPart = Cos(gamma)/gamma;
const Real imagPart = Sin(gamma)/gamma;
A.SetLocal( iLoc, jLoc, C(realPart,imagPart) );
}
}
}
}
void draw_hyperbolic_arc(double x0, double y0, double a, double b,
double f, double g, double c, double s)
{
double e;
e = Atan(b/get_max(x0, y0));
if (f < -e) draw_branch(-180 + e, -e, x0, y0, a, b, f, g, c, s);
if (g > e) draw_branch(e, 180 - e, x0, y0, a, b, f, g, c, s);
}
inline void VectorAngles(const Vector &vec, Angle &angles)
{
if (vec.x == 0.0f && vec.y == 0.0f)
{
if (vec.z > 0.0f)
{
angles.x = -90.0f;
}
else
{
angles.x = 90.0f;
}
}
else
{
angles.x = Rad2Deg(Atan(-vec.z, vec.Length2D()));
angles.y = Rad2Deg(Atan(vec.y, vec.x));
}
}
void Fourier( Matrix<Complex<Real>>& A, Int n )
{
EL_DEBUG_CSE
A.Resize( n, n );
const Real pi = 4*Atan( Real(1) );
const Real nSqrt = Sqrt( Real(n) );
auto fourierFill =
[=]( Int i, Int j ) -> Complex<Real>
{ const Real theta = -2*pi*i*j/n;
return Complex<Real>(Cos(theta),Sin(theta))/nSqrt; };
IndexDependentFill( A, function<Complex<Real>(Int,Int)>(fourierFill) );
}
void Fourier( AbstractBlockDistMatrix<Complex<Real>>& A, Int n )
{
DEBUG_ONLY(CallStackEntry cse("Fourier"))
A.Resize( n, n );
const Real pi = 4*Atan( Real(1) );
const Real nSqrt = Sqrt( Real(n) );
auto fourierFill =
[=]( Int i, Int j ) -> Complex<Real>
{ const Real theta = -2*pi*i*j/n;
return Complex<Real>(Cos(theta),Sin(theta))/nSqrt; };
IndexDependentFill( A, function<Complex<Real>(Int,Int)>(fourierFill) );
}
void UniformHelmholtzGreens( Matrix<Complex<Real>>& A, Int n, Real lambda )
{
EL_DEBUG_CSE
typedef Complex<Real> C;
const Real pi = 4*Atan( Real(1) );
const Real k0 = 2*pi/lambda;
// Generate a list of n uniform samples from the 3D unit ball
Matrix<Real> X(3,n);
for( Int j=0; j<n; ++j )
{
Real x0, x1, x2;
// Sample uniformly from [-1,+1]^3 until a point is drawn from the ball
while( true )
{
x0 = SampleUniform( Real(-1), Real(1) );
x1 = SampleUniform( Real(-1), Real(1) );
x2 = SampleUniform( Real(-1), Real(1) );
const Real radiusSq = x0*x0 + x1*x1 + x2*x2;
if( radiusSq > 0 && radiusSq <= 1 )
break;
}
X(0,j) = x0;
X(1,j) = x1;
X(2,j) = x2;
}
A.Resize( n, n );
for( Int j=0; j<n; ++j )
{
const Real xj0 = X(0,j);
const Real xj1 = X(1,j);
const Real xj2 = X(2,j);
for( Int i=0; i<n; ++i )
{
if( i == j )
{
A(i,j) = 0;
}
else
{
const Real d0 = X(0,i)-xj0;
const Real d1 = X(1,i)-xj1;
const Real d2 = X(2,i)-xj2;
const Real gamma = k0*Sqrt(d0*d0+d1*d1+d2*d2);
const Real realPart = Cos(gamma)/gamma;
const Real imagPart = Sin(gamma)/gamma;
A(i,j) = C(realPart,imagPart);
}
}
}
}
inline void VectorAngles(const Vector &vec, const Vector &up, Angle &angles)
{
Vector left;
Cross(up, vec, left);
left.Normalize();
float len = vec.Length2D();
angles.x = Rad2Deg(Atan(-vec.z, len));
if (len > 0.001f)
{
angles.y = Rad2Deg(Atan(vec.y, vec.x));
angles.z = Rad2Deg(Atan(left.z, ((left.y * vec.x) - (left.x * vec.y))));
}
else
{
angles.y = Rad2Deg(Atan(-left.x, left.y));
angles.z = 0.0f;
}
//NormalizeAngles(angles);
}
void FoxLi( Matrix<Complex<Real>>& A, Int n, Real omega )
{
DEBUG_CSE
typedef Complex<Real> C;
const Real pi = 4*Atan( Real(1) );
const C phi = Sqrt( C(0,omega/pi) );
// Compute Gauss quadrature points and weights
Matrix<Real> d, e;
Zeros( d, n, 1 );
e.Resize( n-1, 1 );
for( Int j=0; j<n-1; ++j )
{
const Real betaInv = 2*Sqrt(1-Pow(j+Real(1),-2)/4);
e(j) = 1/betaInv;
}
Matrix<Real> x, Z;
HermitianTridiagEig( d, e, x, Z, UNSORTED );
auto z = Z( IR(0), ALL );
Matrix<Real> sqrtWeights( z ), sqrtWeightsTrans;
for( Int j=0; j<n; ++j )
sqrtWeights(0,j) = Sqrt(Real(2))*Abs(sqrtWeights(0,j));
herm_eig::Sort( x, sqrtWeights, ASCENDING );
Transpose( sqrtWeights, sqrtWeightsTrans );
// Form the integral operator
A.Resize( n, n );
for( Int j=0; j<n; ++j )
{
for( Int i=0; i<n; ++i )
{
const Real theta = -omega*Pow(x(i)-x(j),2);
const Real realPart = Cos(theta);
const Real imagPart = Sin(theta);
A(i,j) = phi*C(realPart,imagPart);
}
}
// Apply the weighting
DiagonalScale( LEFT, NORMAL, sqrtWeightsTrans, A );
DiagonalScale( RIGHT, NORMAL, sqrtWeightsTrans, A );
}
/* Sets the estimate for the step length and the stance leg angle, assuming that the robot
* is in double stance. This is designed to be called once per step, by the walking finite
* state machine. Returns the step length. Posts other data as STATE variables and to CAN
* bus.*/
float computeHeelStrikeGeometry(void) {
float Slope = 0.0; // Assume flat ground for now
float x, y; // scalar distances, in coordinate system aligned with outer legs
float Phi0 = PARAM_Phi0;
float Phi1 = PARAM_Phi1;
float l = PARAM_l;
float d = PARAM_d;
float qh, q0, q1; // robot joint angles
float stepLength, stepAngle;
qh = mb_io_get_float(ID_MCH_ANGLE); // hip angle - between legs
q0 = mb_io_get_float(ID_MCFO_RIGHT_ANKLE_ANGLE); // outer ankle angle
q1 = mb_io_get_float(ID_MCFI_MID_ANKLE_ANGLE); // inner ankle angle
/* Ranger geometry:
* [x;y] = vector from outer foot virtual center to the inner foot
* virtual center, in a frame that is rotated such that qr = 0
* These functions were determined using computer math. The code can
* be found in:
* templates/Estimator/legAngleEstimator/Derive_Eqns.m
*/
x = l * Sin(qh) - d * Sin(Phi1 - q1 + qh) + d * Sin(Phi0 - q0);
y = l + d * Cos(Phi1 - q1 + qh) - l * Cos(qh) - d * Cos(Phi0 - q0);
stepLength = Sqrt(x * x + y * y); // Distance between two contact points
stepAngle = -(Atan(y / x) + Slope); // angle of the outer legs
mb_io_set_float(ID_EST_LAST_STEP_LENGTH, stepLength);
mb_io_set_float(ID_EST_LAST_STEP_ANGLE, stepAngle);
STATE_lastStepLength = stepLength;
heelStrikeGyroReset(stepAngle); // Reset the gyro estiamte for angles at heel-strike
return stepLength;
}
inline void
MakeDiscreteFourier( Matrix<Complex<R> >& A )
{
#ifndef RELEASE
CallStackEntry entry("MakeDiscreteFourier");
#endif
typedef Complex<R> F;
const int m = A.Height();
const int n = A.Width();
if( m != n )
throw std::logic_error("Cannot make a non-square DFT matrix");
const R pi = 4*Atan( R(1) );
const F nSqrt = Sqrt( R(n) );
for( int j=0; j<n; ++j )
{
for( int i=0; i<m; ++i )
{
const R theta = -2*pi*i*j/n;
A.Set( i, j, Complex<R>(Cos(theta),Sin(theta))/nSqrt );
}
}
}
void FoxLi( ElementalMatrix<Complex<Real>>& APre, Int n, Real omega )
{
DEBUG_CSE
typedef Complex<Real> C;
const Real pi = 4*Atan( Real(1) );
const C phi = Sqrt( C(0,omega/pi) );
DistMatrixWriteProxy<C,C,MC,MR> AProx( APre );
auto& A = AProx.Get();
// Compute Gauss quadrature points and weights
const Grid& g = A.Grid();
DistMatrix<Real,VR,STAR> d(g), e(g);
Zeros( d, n, 1 );
e.Resize( n-1, 1 );
auto& eLoc = e.Matrix();
for( Int iLoc=0; iLoc<e.LocalHeight(); ++iLoc )
{
const Int i = e.GlobalRow(iLoc);
const Real betaInv = 2*Sqrt(1-Pow(i+Real(1),-2)/4);
eLoc(iLoc) = 1/betaInv;
}
DistMatrix<Real,VR,STAR> x(g);
DistMatrix<Real,STAR,VR> Z(g);
HermitianTridiagEig( d, e, x, Z, UNSORTED );
auto z = Z( IR(0), ALL );
DistMatrix<Real,STAR,VR> sqrtWeights( z );
auto& sqrtWeightsLoc = sqrtWeights.Matrix();
for( Int jLoc=0; jLoc<sqrtWeights.LocalWidth(); ++jLoc )
sqrtWeightsLoc(0,jLoc) = Sqrt(Real(2))*Abs(sqrtWeightsLoc(0,jLoc));
herm_eig::Sort( x, sqrtWeights, ASCENDING );
// Form the integral operator
A.Resize( n, n );
DistMatrix<Real,MC,STAR> x_MC( A.Grid() );
DistMatrix<Real,MR,STAR> x_MR( A.Grid() );
x_MC.AlignWith( A );
x_MR.AlignWith( A );
x_MC = x;
x_MR = x;
auto& ALoc = A.Matrix();
auto& x_MCLoc = x_MC.Matrix();
auto& x_MRLoc = x_MR.Matrix();
for( Int jLoc=0; jLoc<A.LocalWidth(); ++jLoc )
{
for( Int iLoc=0; iLoc<A.LocalHeight(); ++iLoc )
{
const Real diff = x_MCLoc(iLoc)-x_MRLoc(jLoc);
const Real theta = -omega*Pow(diff,2);
const Real realPart = Cos(theta);
const Real imagPart = Sin(theta);
ALoc(iLoc,jLoc) = phi*C(realPart,imagPart);
}
}
// Apply the weighting
DistMatrix<Real,VR,STAR> sqrtWeightsTrans(g);
Transpose( sqrtWeights, sqrtWeightsTrans );
DiagonalScale( LEFT, NORMAL, sqrtWeightsTrans, A );
DiagonalScale( RIGHT, NORMAL, sqrtWeightsTrans, A );
}
void PredictBall(Environment *env,int steps)
{
Mydata * p;
p=(Mydata *)env->userData;
Vector3D predictball;
Vector3D ballspeed;
int i=0;
// p->preball.x = p->curball.x + steps*(p->curball.x - p->oldball.x);
// p->preball.y = p->curball.y + steps*(p->curball.y - p->oldball.y);
///z 由来保存球的方向
// p->preball.z = Atan( (p->curball.y - p->oldball.y) , (p->curball.x - p->oldball.x) );
predictball.x = p->curball.x; //赋初值
predictball.y = p->curball.y;
// predictball.z = p->curball.z;
ballspeed.x = p->ballspeed.x ;
ballspeed.y = p->ballspeed.y ;
ballspeed.z = p->ballspeed.z ;
for(i=0; i<steps; i++)
{
predictball.x += ballspeed.x ;
predictball.y += ballspeed.y ;
//处理撞墙
if( predictball.x > FRIGHT-0.7)
{
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
ballspeed.x *=-SPEED_NORMAL; //loose
ballspeed.y *= SPEED_TANGENT;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.x < FLEFT+0.7 )
{
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
ballspeed.x *=-SPEED_NORMAL; //loose
ballspeed.y *= SPEED_TANGENT;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.y < FBOT+0.7 )
{
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
ballspeed.x *= SPEED_TANGENT; //loose
ballspeed.y *=-SPEED_NORMAL;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.y > FTOP -0.7)
{
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
ballspeed.x *= SPEED_TANGENT; //loose
ballspeed.y *=-SPEED_NORMAL;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
if( predictball.x + predictball.y > FRIGHT +FTOP -CORNER)
{
//右上
double vx,vy;
vy=0.7071*ballspeed.y + 0.7071*ballspeed.x; //变换1
vx=-0.7071*ballspeed.y + 0.7071*ballspeed.x;
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
vx *= SPEED_TANGENT; //loose
vy *=-SPEED_NORMAL;
ballspeed.y = 0.7071 * vy - 0.7071 * vx; //变换2
ballspeed.x = 0.7071 * vy + 0.7071 * vx;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
/*
y1 = y * cos() - x * sin();
x1 = y * sin() + x * sin();
*/
}
else if( predictball.x + predictball.y < FLEFT +FBOT+CORNER)
{
//左下
double vx,vy;
vy=0.7071*ballspeed.y + 0.7071*ballspeed.x; //变换1
vx=-0.7071*ballspeed.y + 0.7071*ballspeed.x;
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
vx *= SPEED_TANGENT; //loose
vy *=-SPEED_NORMAL;
ballspeed.y = 0.7071 * vy - 0.7071 * vx; //变换2
ballspeed.x = 0.7071 * vy + 0.7071 * vx;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.x - predictball.y > FRIGHT -FBOT -CORNER)
{
//右下
double vx,vy;
vy=0.7071*ballspeed.y - 0.7071*ballspeed.x; //变换1
vx=0.7071*ballspeed.y + 0.7071*ballspeed.x;
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
vx *= SPEED_TANGENT; //loose
vy *=-SPEED_NORMAL;
ballspeed.y = 0.7071 * vy + 0.7071 * vx; //变换2
ballspeed.x = -0.7071 * vy + 0.7071 * vx;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
else if( predictball.y - predictball.x > FTOP - FLEFT-CORNER)
{
//左上
double vx,vy;
vy=0.7071*ballspeed.y - 0.7071*ballspeed.x; //变换1
vx=0.7071*ballspeed.y + 0.7071*ballspeed.x;
predictball.x -= ballspeed.x ; //retern
predictball.y -= ballspeed.y ;
vx *= SPEED_TANGENT; //loose
vy *=-SPEED_NORMAL;
ballspeed.y = 0.7071 * vy + 0.7071 * vx; //变换2
ballspeed.x = -0.7071 * vy + 0.7071 * vx;
predictball.x += ballspeed.x ; //go on
predictball.y += ballspeed.y ;
}
//处理四角
}
p->preball.x = predictball.x ;
p->preball.y = predictball.y ;
p->preball.z = Atan( ballspeed.y ,ballspeed.x );
}
void Frustum::SetVerticalFovAndAspectRatio(float vFov, float aspectRatio)
{
verticalFov = vFov;
horizontalFov = 2.f * Atan(Tan(vFov*0.5f)*aspectRatio);
}
void Frustum::SetHorizontalFovAndAspectRatio(float hFov, float aspectRatio)
{
horizontalFov = hFov;
verticalFov = 2.f * Atan(Tan(hFov*0.5f)/aspectRatio);
}
All rights reserved.
This file is part of Elemental and is under the BSD 2-Clause License,
which can be found in the LICENSE file in the root directory, or at
http://opensource.org/licenses/BSD-2-Clause
*/
#include "El.hpp"
namespace El {
template<typename Real>
void FoxLi( Matrix<Complex<Real>>& A, Int n, Real omega )
{
DEBUG_ONLY(CSE cse("FoxLi"))
typedef Complex<Real> C;
const Real pi = 4*Atan( Real(1) );
const C phi = Sqrt( C(0,omega/pi) );
// Compute Gauss quadrature points and weights
Matrix<Real> d, e;
Zeros( d, n, 1 );
e.Resize( n-1, 1 );
for( Int j=0; j<n-1; ++j )
{
const Real betaInv = 2*Sqrt(1-Pow(j+Real(1),-2)/4);
e.Set( j, 0, 1/betaInv );
}
Matrix<Real> x, Z;
HermitianTridiagEig( d, e, x, Z, UNSORTED );
auto z = Z( IR(0), ALL );
Matrix<Real> sqrtWeights( z ), sqrtWeightsTrans;
void Si_calib::Terminate()
{
// The Terminate() function is the last function to be called during
// a query. It always runs on the client, it can be used to present
// the results graphically or save the results to file.
//
//calculate the actual energies
//
vector< double > angle;
vector< double > elastic_energy;
vector< double > dE_thickness;
vector< double > E_thickness;
vector< double > dE_loss;
vector< double > E_loss_with;
vector< double > E_loss_without;
double E_max;
double* xxx;
double * tempd= new double(0);
double centrebeam=targ.beam_e_centre(beam_Z,beam_A,ebeam);
dethick=giveme_areal(silicondensity,dethick);//input density in g/cm3 and thickness in um
ethick=giveme_areal(silicondensity,ethick);
for(int i=0;i<16;i++){
angle.push_back(Atan((23.25+(((double)i+1)*1.5))/distance));
xxx = kinetic_lab_calcs_elastic_E(centrebeam,beam_A,targ.targ_A,angle[i]);
elastic_energy.push_back(targ.particle_e_exit(beam_Z,beam_A,xxx[8],TVector3(tan(angle[i]),0,1)));
dE_thickness.push_back(dethick/sin(pi-(deangle+angle[i])));
E_thickness.push_back(ethick/cos(angle[i]));
dE_thickness[i]*=1.02;//you get a bit more thickness when averaging over phi aswell
E_thickness[i]*=1.02;
dE_loss.push_back(elastic_energy[i]-passage(0,beam_Z,beam_A,0,14,28,elastic_energy[i],dE_thickness[i]/1000,tempd));
E_max=egassap(0,beam_Z,beam_A,0,14,28,E_thickness[i]/1000, 0, tempd);
if((elastic_energy[i]-dE_loss[i])<E_max) E_loss_with.push_back(elastic_energy[i]-dE_loss[i]);
else E_loss_with.push_back(E_max);
if((elastic_energy[i])<E_max) E_loss_without.push_back(elastic_energy[i]);
else E_loss_without.push_back(E_max);
cout<<endl<<i+1<<" "<<angle[i]*180/pi<<" "<<dE_loss[i]<<" "<<E_loss_with[i]<<" "<<E_loss_without[i];
}
TF1* b_guas = new TF1("general_peak","gaus(0)");
//const//mean/sigma
b_guas->SetParameter(2,50);
b_guas->SetParLimits(2,5,200);//set sigma limits
//
// Fit the graphs
//
vector< double > dE_sumpeaks;
for(int i=0;i<12;i++){
int maxbin=dE_sums[i]->GetMaximumBin();
int counts=dE_sums[i]->GetBinContent(maxbin);
b_guas->SetParameter(0,counts);
b_guas->SetParLimits(0,counts*0.1,counts*10);
b_guas->SetParameter(1,maxbin);
b_guas->SetParLimits(1,maxbin-50,maxbin+50);
b_guas->SetParameter(2,50);
b_guas->SetRange(maxbin-200,maxbin+200);
dE_sums[i]->Fit(b_guas,"RN");
b_guas->SetRange(b_guas->GetParameter(1)-b_guas->GetParameter(2),b_guas->GetParameter(1)+b_guas->GetParameter(2));
dE_sums[i]->Fit(b_guas,"R");
dE_sumpeaks.push_back(b_guas->GetParameter(1));
}
vector< double > E_withdEpeaks;
for(int i=0;i<32;i++){
int maxbin=Ed_graphs[i]->GetMaximumBin();
int counts=Ed_graphs[i]->GetBinContent(maxbin);
b_guas->SetParameter(0,counts);
b_guas->SetParLimits(0,counts*0.1,counts*10);
b_guas->SetParameter(1,maxbin);
b_guas->SetParLimits(1,maxbin-50,maxbin+50);
b_guas->SetParameter(2,50);
b_guas->SetRange(maxbin-200,maxbin+200);
Ed_graphs[i]->Fit(b_guas,"RN");
b_guas->SetRange(b_guas->GetParameter(1)-b_guas->GetParameter(2),b_guas->GetParameter(1)+b_guas->GetParameter(2));
Ed_graphs[i]->Fit(b_guas,"R");
E_withdEpeaks.push_back(b_guas->GetParameter(1));
}
vector< double > E_peaks;
for(int i=0;i<32;i++){
E_graphs[i]->GetXaxis()->SetRange(Ed_graphs[i]->GetMaximumBin()+80,8000);
int maxbin=E_graphs[i]->GetMaximumBin();
int counts=E_graphs[i]->GetBinContent(maxbin);
b_guas->SetParameter(0,counts);
b_guas->SetParLimits(0,counts*0.1,counts*10);
b_guas->SetParameter(1,maxbin);
b_guas->SetParLimits(1,maxbin-50,maxbin+50);
b_guas->SetParameter(2,50);
b_guas->SetRange(maxbin-200,maxbin+200);
E_graphs[i]->Fit(b_guas,"RN");
b_guas->SetRange(b_guas->GetParameter(1)-b_guas->GetParameter(2),b_guas->GetParameter(1)+b_guas->GetParameter(2));
E_graphs[i]->Fit(b_guas,"R");
E_peaks.push_back(b_guas->GetParameter(1));
}
vector< vector< double > > dE_peaks;
for(int j=0;j<12;j++){
dE_peaks.push_back( vector<double>(0) );
for(int i=0;i<16;i++){
int maxbin=dE_graphs[j][i]->GetMaximumBin();
int counts=dE_graphs[j][i]->GetBinContent(maxbin);
b_guas->SetParameter(0,counts);
b_guas->SetParLimits(0,counts*0.1,counts*10);
b_guas->SetParameter(1,maxbin);
b_guas->SetParLimits(1,maxbin-50,maxbin+50);
b_guas->SetParameter(2,50);
b_guas->SetRange(maxbin-200,maxbin+200);
dE_graphs[j][i]->Fit(b_guas,"RN");
b_guas->SetRange(b_guas->GetParameter(1)-b_guas->GetParameter(2),b_guas->GetParameter(1)+b_guas->GetParameter(2));
dE_graphs[j][i]->Fit(b_guas,"R");
dE_peaks[j].push_back(b_guas->GetParameter(1));
}
}
//
// save the histograms... just because
//
string outtitle=fChain->GetTree()->GetName();
outtitle=outtitle+"_sical.root";
TFile* output_file = new TFile(outtitle.c_str(),"RECREATE");
output_file->cd();
output_file->mkdir("E_no_dE");
output_file->cd("E_no_dE");
for(int i=0;i<32;i++){
E_graphs[i]->GetXaxis()->SetRange(1,8000);
E_graphs[i]->Write();
}
output_file->cd();
output_file->mkdir("E_with_dE");
output_file->cd("E_with_dE");
for(int i=0;i<32;i++){
Ed_graphs[i]->Write();
}
output_file->cd();
output_file->mkdir("dE_sums");
output_file->cd("dE_sums");
for(int i=0;i<12;i++){
dE_sums[i]->Write();
}
output_file->cd();
output_file->mkdir("dE_individual");
output_file->cd("dE_individual");
for(int j=0;j<12;j++){
stringstream ii;
ii<<(j+1);
string titleD="dE_individual/dE"+ii.str();
output_file->mkdir(titleD.c_str());
output_file->cd(titleD.c_str());
for(int i=0;i<16;i++){
dE_graphs[j][i]->Write();
}
output_file->cd("dE_individual");
}
double grad, intercept;
TTree *t = new TTree("si_calib","si_calib");
t->Branch("grad",&grad);
t->Branch("intercept",&intercept);
for(int i=0;i<32;i++){
int j=i;
if(i>15)j=i-16;
grad=(E_loss_without[j]-E_loss_with[j])/(E_peaks[i]-E_withdEpeaks[i]);
intercept=E_withdEpeaks[i]*grad;
intercept=E_loss_with[j]-intercept;
t->Fill();
cout<<endl<<"E"<<i+1<<" "<<" y="<<grad<<"*x+"<<intercept;
}
output_file->mkdir("dE_graphs");
output_file->cd("dE_graphs");
// vector< TGraph* >graph;
for(int i=0;i<12;i++){
double nagrad=(dE_loss[2])/(dE_sumpeaks[i]);
//t->Fill();
cout<<endl<<"dE"<<i+1<<" "<<" y="<<nagrad<<"*x+"<<0<<endl;
stringstream ii;
ii<<(i+1);
string title="dE"+ii.str()+"_graph";
TGraph* graph=new TGraph();
TF1* b_line = new TF1("general_line","pol1(0)");
b_line->SetParameters(nagrad,0);
b_line->SetRange(1,8000);
graph->SetName(title.c_str());
graph->SetMarkerStyle(29);//SetMarkerSize(Size_t msize = 1)
for(int j=0;j<16;j++){
double tester =dE_peaks[i][j]*nagrad;
if(tester>dE_loss[j]*0.98&&tester*0.98<dE_loss[j]){cout<<j<<", ";
for(int k=0;k<10;k++)graph->SetPoint(graph->GetN(),dE_peaks[i][j],dE_loss[j]);
}
}
//graph->SetPoint(graph->GetN(),0,0);
graph->Fit(b_line,"Q");
grad=b_line->GetParameter(0);
intercept=b_line->GetParameter(1);
graph->SetPoint(graph->GetN(),0,0);
cout<<endl<<"dE"<<i+1<<" "<<" y="<<grad<<"*x+"<<intercept;
graph->Fit(b_line,"Q+");
grad=b_line->GetParameter(0);
intercept=b_line->GetParameter(1);
cout<<endl<<"dE"<<i+1<<" "<<" y="<<grad<<"*x+"<<intercept;
t->Fill();
graph->Write();
}
output_file->cd();
output_file->Write();
output_file->Close();
//
//
// if(front_thick>0)ebeam_slow=passage(0,beam_Z,beam_A,backing_compound,backing_Z,backing_A,ebeam,(front_thick)/1000,tempd);
// else ebeam_slow=ebeam;
//
// ebeam_slow=passage(0,beam_Z,beam_A,targ_compound,targ_Z,targ_A,ebeam_slow,(targ_thick/2)/1000,tempd);
//
// if(back_thick>0)ebeam_slow=passage(0,beam_Z,beam_A,backing_compound,backing_Z,backing_A,ebeam_slow,(back_thick)/1000,tempd);
//
//
// double angleE[16];
// double EbeamE[16];
// double thicknessE[16];
// double dEpenetrateE[16];
// double dEnenetrateE[16];
// double* xxx;
// for(int i=0;i<16;i++){
// angleE[i]=TMath::ATan((23.25+((double)i*1.5))/45);
// xxx = kinetic_lab_calcs_elastic_E(ebeam_slow,beam_A,targ_A,angleE[i]);
// EbeamE[i]=xxx[8];
// thicknessE[i]=69.87/TMath::Cos(angleE[i]);
//
// double Einterim=passage(0,3,7,0,14,28,EbeamE[i],18.632,tempd);
//
// dEpenetrateE[i]=Einterim-passage(0,3,7,0,14,28,Einterim,thicknessE[i],tempd);
// dEnenetrateE[i]=EbeamE[i]-passage(0,3,7,0,14,28,EbeamE[i],thicknessE[i],tempd);
// }
// double de_MeV=EbeamE[7]-passage(0,3,7,0,14,28,EbeamE[7],18.632,tempd);
//
//
//
// int j=0;
// for(int i=0;i<44;i++){
// if(i>=32){j=i-32;
// intercept=0;
// grad=de_MeV/dE_graphs[j]->GetMaximumBin();
// }else{
// if(i>15)j=i-16;
// E_graphs[i]->GetXaxis()->GetXaxis()->SetRange(Ed_graphs[i]->GetMaximumBin()+51,8000);
// E_graphs[i]->Smooth(5);
// grad=(dEnenetrateE[j]-dEpenetrateE[j])/(E_graphs[i]->GetMaximumBin()-Ed_graphs[i]->GetMaximumBin());
// intercept=Ed_graphs[i]->GetMaximumBin()*grad;
// intercept=dEpenetrateE[j]-intercept;
// }
//
// if(i==12){grad=0;intercept=0;}
// if(i==42){grad=0;intercept=0;}
// if(i==36){grad=0;intercept=0;}
//
// // t->Fill();
// }
//
// output_file->Write();
// output_file->Close();
}
void PositionAndThrough(Environment *env,int robot,Vector3D pos ,double MAX)
{
Mydata * p;
p=(Mydata *)env->userData;
double anglespeedmax=0; //控制转交速度的变量
double max=MAX;
double Limitedangle=2;
double Limiteddis=0;
double distance;
double turnangle,posangle,vangle;
double dx,dy;
double a=SPEED_A;
double b=SPEED_B;
double v,v1;
double f;
double s=0;
int n=0;
bool face=true; //判断小车是否是正面前进
v= sqrt(p->myspeed[robot].x * p->myspeed[robot].x + p->myspeed[robot].y*p->myspeed[robot].y);
dx = pos.x - p->robot[robot].pos.x ;
dy = pos.y - p->robot[robot].pos.y ;
distance = Distance(p->robot[robot].pos , pos);
posangle = Atan(dy,dx);
turnangle = posangle - p->robot[robot].rotation; //think more!!
ReAngle(turnangle);
if(turnangle > 90)
{
face=false;
turnangle-=180;
}
else if(turnangle < -90)
{
face=false;
turnangle+=180;
}
else
{
face=true;
}
vangle = p->myspeed[robot].z - posangle;
ReAngle(vangle);
if( vangle <-90 || vangle > 90 )
v=-v;
v1=v;
if(distance > Limiteddis)
{
//it is too early to count the steps
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{
//adjust angle
/////////////////测试这一段
if(turnangle > 20 || turnangle < -20)
anglespeedmax = 0;
else if(turnangle > 10 || turnangle < -10)
anglespeedmax = 125;
else if(turnangle > 5 || turnangle < -5)
anglespeedmax = 180;
else
anglespeedmax = 200;
///////////////测试这一段
PAngle(env,robot,posangle,anglespeedmax);
}
else
{
f=max;
if(face)
Velocity(env,robot,f,f);
else
Velocity(env,robot,-f,-f);
}//it is time to rush
}
else
{
}//abserlutely count
}
void PositionAndStop(Environment *env,int robot,Vector3D pos ,double bestangle,double limit)
{
//考虑到可能的 急停和 急快速加速
//特别作了优化
//还有就是 被碰转后的转角过程 不能耽搁时间!!!
//转角是最危险的过程
Mydata * p;
p=(Mydata *)env->userData;
double anglespeedmax=0; //控制转交速度的变量
double vmax=125; //默认的跑位加速度
double Limitedangle=2; //默认减速范围
if( limit < 0.5 )
limit =0.5;
double Limiteddis=limit; //减速范围有一个下限，保证不会来回跑动
double distance; //robot和目标点的距离
double turnangle,posangle,vangle; //转动角度 ，目标点相对robot的角度，速度的绝对角度
double dx,dy; //pos 和robot的坐标差
double a=SPEED_A; //参数
double b=SPEED_B;
double v,v1; //临时保存速度的大小!!!
double f=vmax; //加速度变量
double s=0; //预测的减速位移(路程)
int n=0; //跑位的步数
bool face=true; //判断小车是否是正面前进
v= sqrt(p->myspeed[robot].x * p->myspeed[robot].x + p->myspeed[robot].y*p->myspeed[robot].y);
//临时保存速度的大小!!!
dx = pos.x - p->robot[robot].pos.x ; //pos 和robot的坐标差
dy = pos.y - p->robot[robot].pos.y ;
distance = Distance(p->robot[robot].pos , pos);
posangle = Atan(dy,dx);
turnangle = p->robot[robot].rotation - posangle; //转动角度
ReAngle(turnangle);
if(turnangle > 90)
{
//判断小车是否是正面前进
face=false;
turnangle-=180;
}
else if(turnangle < -90)
{
face=false;
turnangle+=180;
}
else
{
face=true;
}
vangle = p->myspeed[robot].z - p->robot[robot].rotation; //速度的方向和robot正面的夹角
ReAngle(vangle); //主要用于最后控制减速度的大小
if( vangle <-90 || vangle > 90 ) //同时判断v的正负
v=-v;
if(face)
{
//forward 跑位，如果后退的话 就v=0
//设vl,vr=0 还是vl,vr=125 有一个条件有一个临界条件那就是
//v = SPEED_ZERO
if(v < -SPEED_ZERO)
{
Velocity(env,robot,0,0);
return ;
}
}
else if(v > SPEED_ZERO)
{
//back 跑位，如果后退的话 就v=0
Velocity(env,robot,0,0);
return ;
}
v1=v; //v1 is changing while program running
//whlie, v is not
if(distance > Limiteddis )
{
//it is too early to count the steps
//but the Limiteddis should be tested!! to do...
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{
//adjust angle
/////////////////测试这一段
//对于goalie这一段应该特别注意
//发生变向 1.knock the robot,especially the opponent
// 2.knock the wall
// so the anglespeedmax is allowed ++ more!!
if(turnangle > 20 || turnangle < -20)
anglespeedmax = 0;
else if(turnangle > 10 || turnangle < -10)
anglespeedmax = 125;
else if(turnangle > 5 || turnangle < -5)
anglespeedmax = 180;
else
anglespeedmax = 200;
///////////////测试这一段
PAngle(env,robot,posangle,anglespeedmax);
}
else
{
if(face)
Velocity(env,robot,f,f);
else
Velocity(env,robot,-f,-f);
}//it is time to rush
}
else
{
if(distance > 1)
{
//调整角度 return!!!!!!
//radious of robot is about 1.5 ,so the distance is very short
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{
Angle(env,robot,posangle);
return ;
}
}
if(distance < 0.4)
{
//停止并转向 return!!!!!!
//radious of robot is about 1.5 ,so the distance is very short
if( v<0.1 && v>-0.1)
{
//the range of v shoud be tested
Angle(env,robot,bestangle);
return ;
}
}
if(true)
{
vmax=125;
if(face)
{
f=-vmax; //减速度 为 0000000
v1=VelocityOne(v1,-f,-f); //加速一步
s=v1;
do //whether to reduce
{
if(v1 > SPEED_ZERO) //as i said,this is limited
v1=VelocityOne(v1,0,0);
else
v1=VelocityOne(v1,f,f);
s+=v1;
}
while( v1 > 0 );
s-=v1;
if(s < distance)
{
//不满足减速条件加速
Velocity(env,robot,-f,-f);
}
else
{
if(v > SPEED_ZERO)
Velocity(env,robot,0,0);
else
{
v1=VelocityOne(v,f,f); //减速一步
if( v1 < 0 )
{
do //该降低功率了
{
f++; //f=-vmax;
v1 = VelocityOne(v,f,f);
}
while( v1 < distance && f < vmax);
}
Velocity(env,robot,f,f);
}
}
}
else
{
f=vmax; //减速度!!!!!
v1=VelocityOne(v1,-f,-f);
s=v1;
do //whether to reduce
{
if(v1 < -SPEED_ZERO) //as i said,this is limited
v1=VelocityOne(v1,0,0);
else
v1=VelocityOne(v1,f,f);
s+=v1;
}
while( v1 < -0.1 );
s-=v1;
if(s > -distance)
{
//不满足减速条件加速
Velocity(env,robot,-f,-f);
}
else
{
if(v < -SPEED_ZERO)
Velocity(env,robot,0,0);
else
{
v1=VelocityOne(v,f,f); //减速一步
if( v1 > 0 )
{
do //该降低功率了
{
f--; //f=-vmax;
v1 = VelocityOne(v,f,f);
}
while( v1 > -distance && f > -vmax);
}
Velocity(env,robot,f,f);
}
}
}
}
}
}
void PositionAndStopX(Environment *env,int robot,Vector3D pos ,double Xangle,double limit)
{
Mydata * p;
p=(Mydata *)env->userData;
double anglespeedmax=0; //控制转交速度的变量
double vmax=125;
double Limitedangle=2;
if( limit < 2 )
limit =2;
double Limiteddis=limit;
double distance;
double turnangle,posangle,vangle;
double dx,dy;
double a=SPEED_A;
double b=SPEED_B;
double v,v1;
double f=vmax;
double s=0;
int n=0;
bool face=true; //判断小车是否是正面前进
v= sqrt(p->myspeed[robot].x * p->myspeed[robot].x + p->myspeed[robot].y*p->myspeed[robot].y);
dx = pos.x - p->robot[robot].pos.x ;
dy = pos.y - p->robot[robot].pos.y ;
distance = Distance(p->robot[robot].pos , pos);
posangle = Atan(dy,dx);
turnangle = p->robot[robot].rotation - posangle; //think more!!
ReAngle(turnangle);
if(turnangle > 90)
{
face=false;
turnangle-=180;
}
else if(turnangle < -90)
{
face=false;
turnangle+=180;
}
else
{
face=true;
}
vangle = p->myspeed[robot].z - p->robot[robot].rotation;
ReAngle(vangle);
if( vangle <-90 || vangle > 90 )
v=-v;
v1=v;
if(distance > Limiteddis )
{
//it is too early to count the steps
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{
//adjust angle
/////////////////测试这一段
if(turnangle > 20 || turnangle < -20)
anglespeedmax = 0;
else if(turnangle > 10 || turnangle < -10)
anglespeedmax = 125;
else if(turnangle > 5 || turnangle < -5)
anglespeedmax = 180;
else
anglespeedmax = 200;
///////////////测试这一段
PAngle(env,robot,posangle,anglespeedmax);
}
else
{
if(face)
Velocity(env,robot,f,f);
else
Velocity(env,robot,-f,-f);
}//it is time to rush
}
else
{
if(distance > 1)
{
//调整角度 return!!!!!!
if(turnangle > Limitedangle || turnangle < -Limitedangle)
{
Angle(env,robot,posangle);
return ;
}
}
if(distance < 1)
{
//停止并转向 return!!!!!!
if( v<0.5 && v>-0.5)
{
Velocity(env,robot,-Xangle,Xangle);
return ;
}
}
if(true)
{
vmax=125;
if(face)
{
f=-vmax; //减速度 为 0000000
v1=VelocityOne(v1,-f,-f); //加速一步
s=v1;
do //whether to reduce
{
if(v1 > SPEED_ZERO) //as i said,this is limited
v1=VelocityOne(v1,0,0);
else
v1=VelocityOne(v1,f,f);
s+=v1;
}
while( v1 > 0 );
s-=v1;
if(s < distance)
{
//不满足减速条件加速
Velocity(env,robot,-f,-f);
}
else
{
if(v > SPEED_ZERO)
Velocity(env,robot,0,0);
else
{
v1=VelocityOne(v,f,f); //减速一步
if( v1 < 0 )
{
do //该降低功率了
{
f++; //f=-vmax;
v1 = VelocityOne(v,f,f);
}
while( v1 < distance && f < vmax);
}
Velocity(env,robot,f,f);
}
}
}
else
{
f=vmax; //减速度!!!!!
v1=VelocityOne(v1,-f,-f);
s=v1;
do //whether to reduce
{
if(v1 < -SPEED_ZERO) //as i said,this is limited
v1=VelocityOne(v1,0,0);
else
v1=VelocityOne(v1,f,f);
s+=v1;
}
while( v1 < -0.1 );
s-=v1;
if(s > -distance)
{
//不满足减速条件加速
Velocity(env,robot,-f,-f);
}
else
{
if(v < -SPEED_ZERO)
Velocity(env,robot,0,0);
else
{
v1=VelocityOne(v,f,f); //减速一步
if( v1 > 0 )
{
do //该降低功率了
{
f--; //f=-vmax;
v1 = VelocityOne(v,f,f);
}
while( v1 > -distance && f > -vmax);
}
Velocity(env,robot,f,f);
}
}
}
}
}
}
void Angle( Environment *env, int robot,Vector3D pos)
{
Mydata * p;
p=(Mydata *)env->userData;
double speed = 0; //和pangle接轨
double accuracy=1;
double turnangle=0,nextangle=0;
double FF=125; //最大减速度
double angle=0;
angle = Atan(p->robot[robot].pos , pos);
turnangle = angle -p->robot[robot].rotation;
ReAngle(turnangle);
if(turnangle < 1 && turnangle >-1)
{
Velocity(env,robot,0,0);
return ;
}
else if(turnangle < 2 && turnangle >-2)
FF=10;
else if( turnangle >-3 && turnangle < 3)
FF=15;
else if( turnangle >-5 && turnangle < 5)
FF=30;
double v=p->robot[robot].rotation - p->myoldpos[robot].z ;
ReAngle(v);
double v1=v;
double f=0; //相当于减速时,右轮速度，
// int n=0;
bool turnleft=true; //判断小车是否是该向左转
double a=ANGLE_A;
double b=ANGLE_B;
if(turnangle > 90)
{
turnleft=false;
turnangle-=180;
}
else if(turnangle >0)
{
turnleft=true;
}
else if(turnangle > -90)
{
turnleft=false;
}
else
{
turnleft=true;
turnangle+=180;
}
if(turnleft)
{
//
f=-FF;
v1=AngleOne(v1,speed+f,speed-f); //v1+=a *( -b *f-v1);
nextangle+=v1;
do //whether to reduce
{
//收敛!!
v1 =AngleOne(v1,speed-f,speed+f);//+= a *( b *f-v1); // v1
nextangle+=v1;
}
while( v1 > 0 );
nextangle-=v1;
if(nextangle < turnangle)
{
//不满足减速条件 所以 f 取相反数
Velocity(env,robot,speed+f,speed-f);
}
else
{
//reduce
v1 = AngleOne(v,speed-f,speed+f); //v + a *( b *f-v);
if( v1 < 0 )
{
do //该降低功率了
{
f++;
v1 = AngleOne(v,speed-f,speed+f); //v + a *( b *f-v);
}
while( v1 < turnangle && f <FF);
}
Velocity(env,robot,speed-f,speed+f);
}
}
else
{
//
f=FF;
v1=AngleOne(v1,speed+f,speed-f); //v1+=a *( -b *f-v1);
nextangle+=v1;
do //whether to reduce
{
v1 =AngleOne(v1,speed-f,speed+f);//+= a *( b *f-v1); // v1
nextangle+=v1;
}
while( v1 < 0 );
nextangle-=v1;
if(nextangle > turnangle)
{
//不满足减速条件 所以 f 取相反数
Velocity(env,robot,speed+f,speed-f);
}
else
{
//reduce
v1 = AngleOne(v,speed-f,speed+f); //v + a *( b *f-v);
if( v1 > 0 )
{
do //该降低功率了
{
f--;
v1 = AngleOne(v,speed-f,speed+f); //v + a *( b *f-v);
}
while( v1 > turnangle && f >-FF);
}
Velocity(env,robot,speed-f,speed+f);
}
}
}
bool Bridge:: FinalRadius(const double height,
const double theta,
const double alpha,
const bool save)
{
//__________________________________________________________________________
//
// do we save ?
//__________________________________________________________________________
if(save)
{
static vfs & fs = local_fs::instance();
fs.try_remove_file("profile.dat");
}
//__________________________________________________________________________
//
// check possibility
//__________________________________________________________________________
const double omega = lens->omega(alpha);
const double angle = omega+theta;
if(angle>=180)
{
return false;
}
//__________________________________________________________________________
//
// prepare initial conditions
//__________________________________________________________________________
const double Rc = lens->rho(0.0)+height;
const double R0 = lens->rho(alpha);
const double r0 = R0*SinDeg(alpha);
const double z0 = Rc - R0*CosDeg(alpha);
const double drdz = CosDeg(angle)/SinDeg(angle);
Y[1] = r0;
Y[2] = drdz;
//__________________________________________________________________________
//
// do we save ?
//__________________________________________________________________________
auto_ptr<ios::ostream> fp;
if(save)
{
fp.reset(new ios::ocstream("profile.dat",false) );
(*fp)("%g %g\n", r0, z0 );
}
//__________________________________________________________________________
//
// Try to integrate
//__________________________________________________________________________
double reg[3] = { Y[1],0,0 }; // curvature detector
size_t num = 1;
bool success = true;
for(size_t i=NUM_STEPS;i>0;--i)
{
const double z_ini = (i*z0)/NUM_STEPS;
const double z = ((i-1)*z0)/NUM_STEPS;
RK4(z_ini, z);
//______________________________________________________________________
//
// analyze
//______________________________________________________________________
const double r = Y[1];
//______________________________________________________________________
//
// absolute position
//______________________________________________________________________
if( isnan(r) || isinf(r) || r <= 0.0 )
{
success = false;
break;
}
//______________________________________________________________________
//
// Relative position: inside lens ?
//______________________________________________________________________
const double dr = r;
const double dz = Rc-z;
const double dist = Hypotenuse(dr, dz);
const double aa = Rad2Deg(2.0 * Atan( dr / (dz+dist)));
const double ra = lens->rho(aa);
if(dist<ra)
{
success = false;
break;
}
//__________________________________________________________________
//
// test valid curvature
//__________________________________________________________________
if(num<3)
{
reg[num++] = r;
}
else
{
reg[0] = reg[1];
reg[1] = reg[2];
reg[2] = r;
if((reg[1]+reg[1])>= (reg[0]+reg[2]) )
{
success = false;
break;
}
}
if(save && (r<=4*Rc))
{
(*fp)("%g %g\n", r, z );
}
}
return success;
}
