void
MisesMatNl :: give1dStressStiffMtrx(FloatMatrix &answer, MatResponseMode mode, GaussPoint *gp, TimeStep *tStep)
{
    answer.resize(1, 1);
    answer.zero();
    LinearElasticMaterial *lmat = this->giveLinearElasticMaterial();
    double E = lmat->give('E', gp);
    double nlKappa;
    MisesMatNlStatus *status = static_cast< MisesMatNlStatus * >( this->giveStatus(gp) );
    double kappa = status->giveCumulativePlasticStrain();
    double tempKappa = status->giveTempCumulativePlasticStrain();
    double tempDamage = status->giveTempDamage();
    double damage = status->giveDamage();
    answer.at(1, 1) = ( 1 - tempDamage ) * E;
    if ( mode != TangentStiffness ) {
        return;
    }

    if ( tempKappa <= kappa ) { // elastic loading - elastic stiffness plays the role of tangent stiffness
        return;
    }

    // === plastic loading ===
    const FloatArray &stressVector = status->giveTempEffectiveStress();
    double stress = stressVector.at(1);
    answer.at(1, 1) = ( 1. - tempDamage ) * E * H / ( E + H );
    if ( tempDamage > damage ) {
        this->computeCumPlasticStrain(nlKappa, gp, tStep);
        answer.at(1, 1) = answer.at(1, 1) - ( 1 - mm ) * computeDamageParamPrime(nlKappa) * E / ( E + H ) * stress * sgn(stress);
    }
}

bool comparey(const Point &a, const Point &b) {
    return sgn(a.y - b.y) < 0;
}

//------------------------------------------------------------------------
void CVehicleDamageBehaviorBlowTire::Activate(bool activate)
{
  if (activate == m_isActive)
    return;

  if (activate && m_pVehicle->IsDestroyed())
    return;

	if (activate)
	{
		// NOTE: stance and physics position when getting into vehicles is set wrong
		if (!gEnv->pSystem->IsSerializingFile())
			DamagePlayers();
	}

  IVehicleComponent* pComponent = m_pVehicle->GetComponent(m_component.c_str());
  if (!pComponent)
    return;

  IVehiclePart* pPart = pComponent->GetPart(0);
  if (!pPart)
    return;

  // if IVehicleWheel available, execute full damage behavior. if null, only apply effects
  IVehicleWheel* pWheel = pPart->GetIWheel();
  
  if (activate)
  {
    IEntity* pEntity = m_pVehicle->GetEntity();
    IPhysicalEntity* pPhysics = pEntity->GetPhysics();
    const Matrix34& wheelTM = pPart->GetLocalTM(false);
    const SVehicleStatus& status = m_pVehicle->GetStatus();

    if (pWheel)
    { 
      const pe_cargeomparams* pParams = pWheel->GetCarGeomParams();  
            
      // handle destroyed wheel
      pe_params_wheel wheelParams;
      wheelParams.iWheel = pWheel->GetWheelIndex();            
      wheelParams.minFriction = wheelParams.maxFriction = 0.5f * pParams->maxFriction;      
      pPhysics->SetParams(&wheelParams); 
      
      if (IVehicleMovement* pMovement = m_pVehicle->GetMovement())
      { 
        SVehicleMovementEventParams params;
        params.pComponent = pComponent;
        params.iValue = pWheel->GetWheelIndex();
        pMovement->OnEvent(IVehicleMovement::eVME_TireBlown, params);
      }

      if (status.speed > 0.1f)
      {
        // add angular impulse
        pe_action_impulse angImp;
        float amount = m_pVehicle->GetMass() * status.speed * Random(0.25f, 0.45f) * -sgn(wheelTM.GetTranslation().x);
        angImp.angImpulse = pEntity->GetWorldTM().TransformVector(Vec3(0,0,amount));    
        pPhysics->Action(&angImp);
      }
      
      m_aiImmobilizedTimer = m_pVehicle->SetTimer(-1, AI_IMMOBILIZED_TIME*1000, this);  
    }

    if (!gEnv->pSystem->IsSerializingFile())
    {
      // add linear impulse       
      pe_action_impulse imp;
      imp.point = pPart->GetWorldTM().GetTranslation();

      float amount = m_pVehicle->GetMass() * Random(0.1f, 0.15f);

      if (pWheel)
      {
        amount *= max(0.5f, min(10.f, status.speed));

        if (status.speed < 0.1f)
          amount = -0.5f*amount;
      }
      else    
        amount *= 0.5f;

      imp.impulse = pEntity->GetWorldTM().TransformVector(Vec3(0,0,amount));
      pPhysics->Action(&imp);     

      // effect
      IParticleEffect* pEffect = gEnv->p3DEngine->FindParticleEffect(TIRE_BLOW_EFFECT);
      if (pEffect)
      {
        int slot = pEntity->LoadParticleEmitter(-1, pEffect);
        if (slot > -1)
        { 
          float rotation = pWheel ? 0.5f * gf_PI * -sgn(wheelTM.GetTranslation().x) : gf_PI;
          Matrix34 tm = Matrix34::CreateRotationZ(rotation);        
          tm.SetTranslation(wheelTM.GetTranslation());        
          pEntity->SetSlotLocalTM(slot, tm);
        }
      }

			// remove affected decals
//.........这里部分代码省略.........

//Moving function: enter coordinates and the robot does the rest. Keeps track of position by time-based dead reckoning.
//Calibration while running a longer program is possible by assigning negative x- and y-coordinates (the robot will push itself into a corner of the field),
//and then running calibrate (6)
void slitherto(float xgoal, float ygoal, float rgoal) {
	float alfa;
	float beta;
	float _x1 = 0;
	float _y1 = 0;
	float _x1s = 0;
	float _y1s = 0;
	float dirx;
	float diry;

	bool atgoal = false;
	float distancecm;

	float betacor;
	float spd = 30;
	float _x2 = 0;
	float rotspeed;

	atgoal = !((dirx == 1 && xgoal > xcur) || (dirx == -1 && xgoal < xcur) || (diry == 1 && ygoal > ycur) || (diry == -1 && ygoal < ycur) || round(rcur) != rgoal);

	beta = atan2(ygoal-ycur,xgoal-xcur);
	betacor = beta + degreesToRadians(rcur);

	_x1 = cos(betacor);
	_y1 = sin(betacor);

	dirx = sgn(xgoal-floor(xcur));
	diry = sgn(ygoal-floor(ycur));

	time1[T1] = 0;

	while(atgoal == false) {
		{
			if ((dirx == 1 && xgoal > xcur) || (dirx == -1 && xgoal < xcur)) {
				_x1s = 1;
			}
			else {
				_x1s = 0;
			}
			if ((diry == 1 && ygoal > ycur) || (diry == -1 && ygoal < ycur)) {
				_y1s = 1;
			}
			else {
				_y1s = 0;
			}

			if (round(rcur) < rgoal - 5 && (_x1s || _y1s))
				_x2 = 1;
			else if (round(rcur) > rgoal + 5 && (_x1s || _y1s))
				_x2 = -1;
			else if (round(rcur) < rgoal && (_x1s || _y1s))
				_x2 = 0.3;
			else if (round(rcur) > rgoal && (_x1s || _y1s))
				_x2 = -0.3;
			else if (round(rcur) < rgoal && !(_x1s || _y1s))
				_x2 = 1;
			else if (round(rcur) > rgoal && !(_x1s || _y1s))
				_x2 = -1;

			if (beta/1.571 - floor(beta/1.571) > 0.785) //Oh radians....
				alfa = 1.571 - (beta/1.571 - floor(beta/1.571));
			else
				alfa = beta/1.571 - floor(beta/1.571);

			wait1Msec(10);

			distancecm = abs(((1-C_D)/(-0.785*alfa) + 1)*((d0_1s_1+d0_1s_2)/2)*C_T*time1[T1]*0.001);

			xcur += distancecm*cos(beta);
			ycur += distancecm*sin(beta);

			HTACreadAllAxes(HTAC, xacc, yacc, zacc);
			xacc -= xcal;
			yacc -= ycal;
			zacc -= zcal;
			writeDebugStreamLine("%d,%d",xacc,yacc);

			rotspeed = HTGYROreadRot(gyro); //Read the current rotation speed
			rcur += rotspeed * time1[T1]*0.001; //Magic
			nxtDisplayCenteredBigTextLine(3, "%2.0f", rcur); //Display our current heading on the screen

			if (abs(xacc) > 120 || abs(yacc) > 120) {
				PlaySound(soundBeepBeep);

				//Move in opposite direction
				motor[fr] = spd*(_x1*_x1s-_y1*_y1s);
				motor[br] = spd*(-_x1*_x1s-_y1*_y1s);
				motor[bl] = spd*(-_x1*_x1s+_y1*_y1s);
				motor[fl] = spd*(_x1*_x1s+_y1*_y1s);

				time1[T1] = 0;

				while (time1[T1] < 1000) {
					time1[T2] = 0;

					wait1Msec(10);

//.........这里部分代码省略.........

void ai09::NormalPlayAtt ( void )
{
	ManageAttRoles ( );
	
	debugDraw=true;
	recievePass(dmf,PointOnConnectingLine(ball.Position, Vec2(side*field_width, 0), 2500));
	debugDraw=false;
	
	if (oneTouchType[attack]==allaf) {
		ERRTSetObstacles ( attack , false , true , true , true );
		OwnRobot[attack].face(Vec2(-side*field_width, 0));
		//OwnRobot[robot_num].target.Angle=-90;
		ERRTNavigate2Point ( attack , allafPos[attack] ,0 , 100,&VELOCITY_PROFILE_MAMOOLI);
		if (timer.time()>2.5) {
			oneTouchType[attack] = oneTouch;
		}
        
        activeShootTimer.start();
	}
    
    else
    {
        float ballReachTimeTmp = calculateBallRobotReachTime(attack, &VELOCITY_PROFILE_MAMOOLI) * 1.5;
        TVec2 ballReachPlace = predictBallForwardAI(ballReachTimeTmp);
        float ballGoalDot = (Dot(Normalize(Vec2(ball.velocity.x, ball.velocity.y)), Normalize(Vec2(-side*field_width, 0)-ballReachPlace)));
        if ( 0)//ballGoalDot > -0.6 && ballGoalDot < 0.7 && ball.velocity.magnitude > 900 )
        {
            float passAngle = ball.velocity.direction;
            tech_circle(attack, passAngle, 1, 0, 1, 0, 0, 1);
        }
        else
        {
            TVec2 openAngle = calculateOpenAngleToGoal(ball.Position, attack);
            
            bool mid1Reached = OwnRobot[mid1].State.velocity.magnitude < 50;
            bool mid2Reached = OwnRobot[mid2].State.velocity.magnitude < 50;
            
            bool mid1DisOk = DIS(OwnRobot[mid1].State.Position, ball.Position) > 2000;
            bool mid2DisOk = DIS(OwnRobot[mid2].State.Position, ball.Position) > 2000;
            
            bool mid1Seen = OwnRobot[mid1].State.seenState != CompletelyOut;
            bool mid2Seen = OwnRobot[mid2].State.seenState != CompletelyOut;
            
            bool mid1Suitable = mid1Seen && mid1Reached && mid1DisOk;
            bool mid2Suitable = mid2Seen && mid2Reached && mid2DisOk;
            
            if (mid1Suitable && mid2Suitable)
            {
                if(-side*OwnRobot[mid1].State.Position.X > -side*OwnRobot[mid2].State.Position.X)
                    mid2Suitable = false;
                else
                    mid1Suitable = false;
            }

	if ( openAngle.Y < 2 && (mid1Suitable||mid2Suitable) && (findKickerOpp(-1)==-1) )//&& ( ball.Position.X * side < -2300 ) && ( fabs ( ball.Position.Y ) > 1800 ) )
	{
        
		//float passAngle = AngleWith ( OwnRobot[randomParam<0.3?dmf:(randomParam<0.6?rmf:lmf)].State.Position , ball.Position );
        float passAngle = AngleWith ( Vec2 ( -side*1700 , -sgn ( ball.Position.Y ) * 1700 ) , ball.Position );
        float chip_pow = 40;
        
        if ( mid1Suitable )
        {
            passAngle = AngleWith ( OwnRobot[mid1].State.Position , ball.Position );
            chip_pow = 50.0 * DIS(OwnRobot[mid1].State.Position, ball.Position) / 2000.0f;
        }
        else if ( mid2Suitable )
        {
            passAngle = AngleWith ( OwnRobot[mid2].State.Position , ball.Position );
            chip_pow = 50.0 * DIS(OwnRobot[mid2].State.Position, ball.Position) / 2000.0f;
        }
        else
        {
            passAngle = AngleWith ( Vec2 ( -side*1700 , -sgn ( ball.Position.Y ) * 1700 ) , ball.Position );
            chip_pow = 1;
        }
		tech_circle(attack, passAngle, 0, chip_pow, 1, 0, 0, 1);
	}
	else {
        
        float shootAngle;
        if ( openAngle.Y > 10 )
            shootAngle = NormalizeAngle( 180+openAngle.X);
        else
            shootAngle = AngleWith ( Vec2 ( -side*field_width , 0 ) , ball.Position );
        
		float shoot_pow = 80 - OwnRobot[attack].State.velocity.magnitude * 0.01;


        //if ( openAngle.Y < 2 )
        //    shoot_pow = 0;
		if (DIS(OwnRobot[attack].State.Position,ball.Position) > 400 ) {
			shoot_pow = 1;
            activeShootTimer.start();
		}
		else if (goal_blocked(ball.Position, 200, 90)) {
			shoot_pow = 1;
		}
        else
        {
//.........这里部分代码省略.........

void CXInputDevice::Update(bool bFocus)
{
	FUNCTION_PROFILER( GetISystem(),PROFILE_INPUT );

	DEBUG_CONTROLLER_RENDER_BUTTON_ACTION;

	const bool wasConnected = m_connected;
	const bool connected = g_bConnected[m_deviceNo];
	const bool disconnecting = (wasConnected && !connected);

	UpdateConnectedState(connected);

	float frameTime = gEnv->pTimer->GetFrameTime();
	float now = gEnv->pTimer->GetFrameStartTime().GetSeconds();
	if((m_fVibrationTimer && m_fVibrationTimer < now) || g_pInputCVars->i_forcefeedback == 0 ||
		gEnv->pSystem->IsPaused() || frameTime<0.001f)
	{
		m_fVibrationTimer = 0;
		SetVibration(0, 0, 0.0f);
	}

	// Force inputs to get sent out when we're disconnecting or
	// we can get stuck with thumbsticks in a non-neutral state
	if (disconnecting)
	{
		m_forceResendSticks = true;
	}

	// interpret input
	if ((m_connected && bFocus) || disconnecting)
	{
		XINPUT_STATE state;
		memset( &state, 0, sizeof(XINPUT_STATE) );
		if ( ERROR_SUCCESS != XInputGetState(m_deviceNo, &state) && !disconnecting)
			return;

		if (state.dwPacketNumber != m_state.dwPacketNumber)
		{
			SInputEvent event;
			SInputSymbol*	pSymbol = 0;
			
			event.deviceIndex = (uint8)m_deviceNo;

      if (g_pInputCVars->i_xinput_deadzone_handling > 0)
      {
			  {
				  Vec2 d( state.Gamepad.sThumbLX, state.Gamepad.sThumbLY );

				  FixDeadzone( d );

				  state.Gamepad.sThumbLX = d[0];
				  state.Gamepad.sThumbLY = d[1];
			  }

			  {
				  Vec2 d( state.Gamepad.sThumbRX, state.Gamepad.sThumbRY );

				  FixDeadzone( d );

				  state.Gamepad.sThumbRX = d[0];
				  state.Gamepad.sThumbRY = d[1];
			  }
      }
      else
      {
        const float INV_VALIDRANGE = (1.0f / (INPUT_MAX - m_fDeadZone));

        // make the inputs move smoothly out of the deadzone instead of snapping straight to m_fDeadZone
        float fraction=max((float)abs(state.Gamepad.sThumbLX) - m_fDeadZone, 0.f) * INV_VALIDRANGE; 
        float oldVal=state.Gamepad.sThumbLX;
        state.Gamepad.sThumbLX = fraction * INPUT_MAX * sgn(state.Gamepad.sThumbLX); 

        fraction = max((float)abs(state.Gamepad.sThumbLY) - m_fDeadZone, 0.f) * INV_VALIDRANGE; 
        oldVal = state.Gamepad.sThumbLY;
        state.Gamepad.sThumbLY = fraction * INPUT_MAX * sgn(state.Gamepad.sThumbLY);

        // make the inputs move smoothly out of the deadzone instead of snapping straight to m_fDeadZone
        fraction=max((float)abs(state.Gamepad.sThumbRX) - m_fDeadZone, 0.f) * INV_VALIDRANGE; 
        oldVal=state.Gamepad.sThumbRX;
        state.Gamepad.sThumbRX = fraction * INPUT_MAX * sgn(state.Gamepad.sThumbRX); 

        fraction = max((float)abs(state.Gamepad.sThumbRY) - m_fDeadZone, 0.f) * INV_VALIDRANGE; 
        oldVal = state.Gamepad.sThumbRY;
        state.Gamepad.sThumbRY = fraction * INPUT_MAX * sgn(state.Gamepad.sThumbRY);

      }

			// compare new values against cached value and only send out changes as new input
			WORD buttonsChange = m_state.Gamepad.wButtons ^ state.Gamepad.wButtons;
			if (buttonsChange)
			{	
				for (int i = 0; i < 16; ++i)
				{
					uint32 id = (1 << i);
					if (buttonsChange & id && (pSymbol = DevSpecIdToSymbol(id)))
					{
						pSymbol->PressEvent((state.Gamepad.wButtons & id) != 0);
						pSymbol->AssignTo(event);
						event.deviceType = eIDT_Gamepad;
						GetIInput().PostInputEvent(event);
//.........这里部分代码省略.........

int ElastomericBearingBoucWen2d::update()
{
    // get global trial displacements and velocities
    const Vector &dsp1 = theNodes[0]->getTrialDisp();
    const Vector &dsp2 = theNodes[1]->getTrialDisp();
    const Vector &vel1 = theNodes[0]->getTrialVel();
    const Vector &vel2 = theNodes[1]->getTrialVel();
    
    static Vector ug(6), ugdot(6), uldot(6), ubdot(3);
    for (int i=0; i<3; i++)  {
        ug(i)   = dsp1(i);  ugdot(i)   = vel1(i);
        ug(i+3) = dsp2(i);  ugdot(i+3) = vel2(i);
    }
    
    // transform response from the global to the local system
    ul.addMatrixVector(0.0, Tgl, ug, 1.0);
    uldot.addMatrixVector(0.0, Tgl, ugdot, 1.0);
    
    // transform response from the local to the basic system
    ub.addMatrixVector(0.0, Tlb, ul, 1.0);
    ubdot.addMatrixVector(0.0, Tlb, uldot, 1.0);
    
    // 1) get axial force and stiffness in basic x-direction
    theMaterials[0]->setTrialStrain(ub(0), ubdot(0));
    qb(0) = theMaterials[0]->getStress();
    kb(0,0) = theMaterials[0]->getTangent();
    
    // 2) calculate shear force and stiffness in basic y-direction
    // get displacement increment (trial - commited)
    double delta_ub = ub(1) - ubC(1);
    if (fabs(delta_ub) > 0.0)  {
        
        // get yield displacement
        double uy = qYield/k0;
        
        // calculate hysteretic evolution parameter z using Newton-Raphson
        int iter = 0;
        double zAbs, tmp1, f, Df, delta_z;
        do  {
            zAbs = fabs(z);
            if (zAbs == 0.0)    // check because of negative exponents
                zAbs = DBL_EPSILON;
            tmp1 = gamma + beta*sgn(z*delta_ub);
            
            // function and derivative
            f  = z - zC - delta_ub/uy*(A - pow(zAbs,eta)*tmp1);
            Df = 1.0 + delta_ub/uy*eta*pow(zAbs,eta-1.0)*sgn(z)*tmp1;
            
            // issue warning if derivative Df is zero
            if (fabs(Df) <= DBL_EPSILON)  {
                opserr << "WARNING: ElastomericBearingBoucWen2d::update() - "
                    << "zero derivative in Newton-Raphson scheme for "
                    << "hysteretic evolution parameter z.\n";
                return -1;
            }
            
            // advance one step
            delta_z = f/Df;
            z -= delta_z;
            iter++;
        } while ((fabs(delta_z) >= tol) && (iter < maxIter));
        
        // issue warning if Newton-Raphson scheme did not converge
        if (iter >= maxIter)   {
            opserr << "WARNING: ElastomericBearingBoucWen2d::update() - "
                << "did not find the hysteretic evolution parameter z after "
                << iter << " iterations and norm: " << fabs(delta_z) << endln;
            return -2;
        }
        
        // get derivative of hysteretic evolution parameter * uy
        dzdu = A - pow(fabs(z),eta)*(gamma + beta*sgn(z*delta_ub));
        // set shear force
        qb(1) = qYield*z + k2*ub(1) + k3*sgn(ub(1))*pow(fabs(ub(1)),mu);
        // set tangent stiffness
        kb(1,1) = k0*dzdu + k2 + k3*mu*pow(fabs(ub(1)),mu-1.0);
    }
    
    // 3) get moment and stiffness about basic z-direction
    theMaterials[1]->setTrialStrain(ub(2), ubdot(2));
    qb(2) = theMaterials[1]->getStress();
    kb(2,2) = theMaterials[1]->getTangent();
    
    return 0;
}

task drive()
{

	int Joy1X;
	int Joy1Xmve;
	int Joy1Y;
	int Joy1Ymve;
	int L_side_motor;
	int R_side_motor;

	float Gain2 = 0.07;
	float Gain3 = 0.1;

	while (true)
	{

		getJoystickSettings(joystick);

		Joy1X = joystick.joy1_x1;  //Variables detect the value of the joystick x and y axis.
		Joy1Y = joystick.joy1_y1;

		//This is the deadband for the y axis.
		if( (abs(Joy1Y) < deadband))
		{
			Joy1Ymve = 0;
		}
		else
		{
			if(abs(Joy1Y) > deadband) //Checks if the absolute value of the joystick y axis is grater than the deadband
				Joy1Ymve = Joy1Y - (sgn(Joy1Y) * deadband);//subtracts the deadband from the joystic value to start the power at zero
		}

		//This is the deadband for the x axis
		if( (abs(Joy1X) < deadband))
		{
			Joy1Xmve = 0;
		}
		else
		{
			if(abs(Joy1X) > deadband) //Checks if the absolute value of the joystick x axis is grater than the deadband
				Joy1Xmve = Joy1X - (sgn(Joy1X) * deadband);//subtracts the deadband from the joystic value to start the power at zero
		}

		//where the stick shaping magic happens.
		Joy1Ymve = ((Joy1Ymve * Gain2) * (abs(Joy1Ymve) * Gain3));
		Joy1Xmve = ((Joy1Xmve * Gain2) * (abs(Joy1Xmve) * Gain3));
		//this equaion can also be phrased as Y = k1X^2 + k2X.

		//Provisions for turning.
		L_side_motor = Joy1Ymve + Joy1Xmve;
		R_side_motor = Joy1Ymve - Joy1Xmve;
		//this value can be greater than |100| so greater values will need to be reduced to |100|

		if ( abs(L_side_motor) > 100)
		{
			L_side_motor = sgn(L_side_motor) * 100;

		}

		if ( abs(R_side_motor) > 100)
		{
			R_side_motor = sgn(R_side_motor) * 100;
		}

		//now we set the power of the motors.

		motor[L_Motor] = L_side_motor;
		motor[R_Motor] = R_side_motor;
	}
}

static double soft(double lambda, double z)
{
    if (fabs(z) > lambda) return(sgn(z) * (fabs(z) - lambda));
    else return(0);
}

//------------------------------------------------------------------------
void CScriptControlledPhysics::OnPostStep(EventPhysPostStep *pPostStep)
{
    pe_action_set_velocity av;

    const bool moving = m_moving;
    const bool rotating = m_rotating;

    const float deltaTime = pPostStep->dt;

    if (m_moving)
        {
            const Vec3 current = pPostStep->pos;
            const Vec3 target = m_moveTarget;
            const Vec3 delta = target - current;
            const float distanceSq = delta.len2();

            if (distanceSq <= sqr(0.025f) || (delta.dot(m_lastVelocity) < 0.0f))
                {
                    m_speed = 0.0f;
                    m_moving = false;
                    m_lastVelocity.zero();

                    pPostStep->pos = target;

                    av.v = ZERO;
                }
            else
                {
                    float velocity = m_speed;
                    float acceleration = m_acceleration;
                    Vec3 direction = delta;
                    const float distanceToEnd = direction.NormalizeSafe();

                    // Accelerate
                    velocity = std::min(velocity + acceleration * deltaTime, m_maxSpeed);

                    // Calculate acceleration and time needed to stop
                    const float accelerationNeededToStop = (-(velocity*velocity) / distanceToEnd) / 2;
                    if (fabsf(accelerationNeededToStop) > 0.0f)
                        {
                            const float timeNeededToStop = sqrtf((distanceToEnd / 0.5f) / fabsf(accelerationNeededToStop));

                            if (timeNeededToStop < m_stopTime)
                                {
                                    acceleration = accelerationNeededToStop;
                                }
                        }

                    // Prevent overshooting
                    if ((velocity * deltaTime) > distanceToEnd)
                        {
                            const float multiplier = distanceToEnd / fabsf(velocity * deltaTime);
                            velocity *= multiplier;
                            acceleration *= multiplier;
                        }

                    m_acceleration = acceleration;
                    m_speed = velocity;

                    m_lastVelocity = direction * velocity;
                    av.v = direction * velocity;
                }
        }

    if (m_rotating)
        {
            Quat current=pPostStep->q;
            Quat target=m_rotationTarget;

            Quat rotation=target*!current;
            float angle=cry_acosf(CLAMP(rotation.w, -1.0f, 1.0f))*2.0f;
            float original=angle;
            if (angle>gf_PI)
                angle=angle-gf_PI2;
            else if (angle<-gf_PI)
                angle=angle+gf_PI2;

            if (cry_fabsf(angle)<0.01f)
                {
                    m_rotationSpeed=0.0f;
                    m_rotating=false;
                    pPostStep->q=m_rotationTarget;
                    av.w=ZERO;
                }
            else
                {
                    float a=m_rotationSpeed/m_rotationStopTime;
                    float d=m_rotationSpeed*m_stopTime-0.5f*a*m_rotationStopTime*m_rotationStopTime;

                    if (cry_fabsf(angle)<d+0.001f)
                        m_rotationAcceleration=(angle-m_rotationSpeed*m_rotationStopTime)/(m_rotationStopTime*m_rotationStopTime);

                    m_rotationSpeed=m_rotationSpeed+sgn(angle)*m_rotationAcceleration*deltaTime;
                    if (cry_fabsf(m_rotationSpeed*deltaTime)>cry_fabsf(angle))
                        m_rotationSpeed=angle/deltaTime;
                    else if (cry_fabsf(m_rotationSpeed)<0.001f)
                        m_rotationSpeed=sgn(m_rotationSpeed)*0.001f;
                    else if (cry_fabsf(m_rotationSpeed)>=m_rotationMaxSpeed)
                        {
//.........这里部分代码省略.........

void DetailsTable::createCoordinatesTable(SkyObject *obj, const KStarsDateTime &ut, GeoLocation *geo)
{
    clearContents();

    QTextCursor cursor = m_Document->rootFrame()->firstCursorPosition();

    // Set column width constraints
    QVector<QTextLength> constraints;
    constraints << QTextLength(QTextLength::PercentageLength, 25)
                << QTextLength(QTextLength::PercentageLength, 25)
                << QTextLength(QTextLength::PercentageLength, 25)
                << QTextLength(QTextLength::PercentageLength, 25);
    m_TableFormat.setColumnWidthConstraints(constraints);

    // Insert table & row containing table name
    QTextTable *table = cursor.insertTable(4, 4, m_TableFormat);
    table->mergeCells(0, 0, 1, 4);
    QTextBlockFormat centered;
    centered.setAlignment(Qt::AlignCenter);
    table->cellAt(0, 0).firstCursorPosition().setBlockFormat(centered);
    table->cellAt(0, 0).firstCursorPosition().insertText(i18n("Coordinates"), m_TableTitleCharFormat);

    //Coordinates Section:
    //Don't use KLocale::formatNumber() for the epoch string,
    //because we don't want a thousands-place separator!
    QString sEpoch = QString::number(ut.epoch(), 'f', 1);
    //Replace the decimal point with localized decimal symbol
    sEpoch.replace('.', KGlobal::locale()->decimalSymbol());

    table->cellAt(1, 0).firstCursorPosition().insertText(i18n("RA (%1):", sEpoch), m_ItemNameCharFormat);
    table->cellAt(1, 0).firstCursorPosition().setBlockFormat(centered);
    table->cellAt(1, 1).firstCursorPosition().insertText(obj->ra().toHMSString(), m_ItemValueCharFormat);

    table->cellAt(2, 0).firstCursorPosition().insertText(i18n("Dec (%1):", sEpoch), m_ItemNameCharFormat);
    table->cellAt(2, 0).firstCursorPosition().setBlockFormat(centered);
    table->cellAt(2, 1).firstCursorPosition().insertText(obj->dec().toDMSString(), m_ItemValueCharFormat);

    table->cellAt(3, 0).firstCursorPosition().insertText(i18n("Hour angle:"), m_ItemNameCharFormat);
    table->cellAt(3, 0).firstCursorPosition().setBlockFormat(centered);
    //Hour Angle can be negative, but dms HMS expressions cannot.
    //Here's a kludgy workaround:
    dms lst = geo->GSTtoLST(ut.gst());
    dms ha(lst.Degrees() - obj->ra().Degrees());
    QChar sgn('+');
    if(ha.Hours() > 12.0)
    {
        ha.setH(24.0 - ha.Hours());
        sgn = '-';
    }
    table->cellAt(3, 1).firstCursorPosition().insertText(QString("%1%2").arg(sgn).arg(ha.toHMSString()), m_ItemValueCharFormat);

    table->cellAt(1, 2).firstCursorPosition().insertText(i18n("Azimuth:"), m_ItemNameCharFormat);
    table->cellAt(1, 2).firstCursorPosition().setBlockFormat(centered);
    table->cellAt(1, 3).firstCursorPosition().insertText(obj->az().toDMSString(), m_ItemValueCharFormat);

    table->cellAt(2, 2).firstCursorPosition().insertText(i18n("Altitude:"), m_ItemNameCharFormat);
    table->cellAt(2, 2).firstCursorPosition().setBlockFormat(centered);
    dms a;
    if(Options::useAltAz())
    {
        a = obj->alt();
    }

    else
    {
        a = obj->altRefracted();
    }
    table->cellAt(2, 3).firstCursorPosition().insertText(a.toDMSString(), m_ItemValueCharFormat);

    table->cellAt(3, 2).firstCursorPosition().insertText(i18n("Airmass:"), m_ItemNameCharFormat);
    table->cellAt(3, 2).firstCursorPosition().setBlockFormat(centered);
    //Airmass is approximated as the secant of the zenith distance,
    //equivalent to 1./sin(Alt).  Beware of Inf at Alt=0!
    QString aMassStr;
    if(obj->alt().Degrees() > 0.0)
    {
        aMassStr = KGlobal::locale()->formatNumber(1./sin(obj->alt().radians() ), 2);
    }

    else
    {
        aMassStr = "--";
    }
    table->cellAt(3, 3).firstCursorPosition().insertText(aMassStr, m_ItemValueCharFormat);

    // Restore the position and other time-dependent parameters
    obj->recomputeCoords(ut, geo);
}

void CMountedGunController::Update(EntityId mountedGunID, float frameTime)
{
    CRY_ASSERT_MESSAGE(m_pControlledPlayer, "Controlled player not initialized");

    CItem* pMountedGun = static_cast<CItem*>(gEnv->pGame->GetIGameFramework()->GetIItemSystem()->GetItem(mountedGunID));

    bool canUpdateMountedGun = (pMountedGun != NULL) && (pMountedGun->GetStats().mounted);

    if (canUpdateMountedGun)
        {
            IMovementController * pMovementController = m_pControlledPlayer->GetMovementController();
            assert(pMovementController);

            SMovementState info;
            pMovementController->GetMovementState(info);

            IEntity* pMountedGunEntity = pMountedGun->GetEntity();
            const Matrix34& lastMountedGunWorldTM = pMountedGunEntity->GetWorldTM();

            Vec3 desiredAimDirection = info.aimDirection.GetNormalized();

            // AI can switch directions too fast, prevent snapping
            if(!m_pControlledPlayer->IsPlayer())
                {
                    const Vec3 currentDir = lastMountedGunWorldTM.GetColumn1();
                    const float dot = clamp(currentDir.Dot(desiredAimDirection), -1.0f, 1.0f);
                    const float reqAngle = cry_acosf(dot);
                    const float maxRotSpeed = 2.0f;
                    const float maxAngle = frameTime * maxRotSpeed;
                    if(fabs(reqAngle) > maxAngle)
                        {
                            const Vec3 axis = currentDir.Cross(desiredAimDirection);
                            if(axis.GetLengthSquared() > 0.001f) // current dir and new dir are enough different
                                {
                                    desiredAimDirection = currentDir.GetRotated(axis.GetNormalized(),sgn(reqAngle)*maxAngle);
                                }
                        }
                }

            bool isUserClient = m_pControlledPlayer->IsClient();

            IEntity* pMountedGunParentEntity = pMountedGunEntity->GetParent();
            IVehicle *pVehicle = NULL;
            if(pMountedGunParentEntity && m_pControlledPlayer)
                pVehicle = m_pControlledPlayer->GetLinkedVehicle();

            CRecordingSystem* pRecordingSystem = g_pGame->GetRecordingSystem();

            //For client update always, for others only when there is notable change
            if (!pVehicle && (isUserClient || (!desiredAimDirection.IsEquivalent(lastMountedGunWorldTM.GetColumn1(), 0.003f))))
                {
                    Quat rotation = Quat::CreateRotationVDir(desiredAimDirection, 0.0f);
                    pMountedGunEntity->SetRotation(rotation);

                    if (isUserClient && pRecordingSystem)
                        {
                            // Only record the gun position if you're using the gun.
                            pRecordingSystem->OnMountedGunRotate(pMountedGunEntity, rotation);
                        }
                }

            const Vec3 vInitialAimDirection = GetMountDirection(pMountedGun, pMountedGunParentEntity);
            assert( vInitialAimDirection.IsUnit() );

            //Adjust gunner position and animations
            UpdateGunnerLocation(pMountedGun, pMountedGunParentEntity, vInitialAimDirection);

            const float aimrad = Ang3::CreateRadZ(Vec2(vInitialAimDirection),Vec2(-desiredAimDirection));
            const float pitchLimit = sin_tpl(DEG2RAD(30.0f));
            const float animHeight = fabs_tpl(clamp(desiredAimDirection.z * (float)__fres(pitchLimit), -1.0f, 1.0f));

            const float aimUp = (float)__fsel(-desiredAimDirection.z, 0.0f, animHeight);
            const float aimDown = (float)__fsel(desiredAimDirection.z, 0.0f, animHeight);

            if (pRecordingSystem)
                {
                    pRecordingSystem->OnMountedGunUpdate(m_pControlledPlayer, aimrad, aimUp, aimDown);
                }

            if(!m_pControlledPlayer->IsThirdPerson())
                {
                    UpdateFirstPersonAnimations(pMountedGun, desiredAimDirection);
                }

            if(m_pMovementAction)
                {
                    const float aimUpParam = aimUp;
                    const float aimDownParam = aimDown;
                    const float aimMovementParam = CalculateAnimationTime(aimrad);

                    m_pMovementAction->SetParam(MountedGunCRCs.aimUpParam, aimUpParam);
                    m_pMovementAction->SetParam(MountedGunCRCs.aimDownParam, aimDownParam);
                    m_pMovementAction->SetParam(MountedGunCRCs.aimMovementParam, aimMovementParam);
                }

            UpdateIKMounted(pMountedGun);
        }
}

//.........这里部分代码省略.........
                    arti1_primed = TRUE;
                else
                    arti_cursed = TRUE;
            }
            if (otmp->otyp == BELL_OF_OPENING && (moves - otmp->age) < 5L) {
                /* you rang it recently */
                if (!otmp->cursed)
                    arti2_primed = TRUE;
                else
                    arti_cursed = TRUE;
            }
        }

        if (arti_cursed) {
            pline("The invocation fails!");
            if (Hallucination)
                pline("At least one of your heirlooms is in a tizzy!");
            else
                pline("At least one of your artifacts is cursed...");
        } else if (arti1_primed && arti2_primed) {
            unsigned soon = (unsigned)dice(2, 6); /* time til next intervene */

            /* successful invocation */
            mkinvokearea();
            u.uevent.invoked = 1;
            historic_event(FALSE, TRUE, "performed the invocation.");
            /* in case you haven't killed the Wizard yet, behave as if you just
               did */
            u.uevent.udemigod = 1;      /* wizdead() */
            if (!u.udg_cnt || u.udg_cnt > soon)
                u.udg_cnt = soon;
        } else {        /* at least one artifact not prepared properly */
            pline("You have a feeling that something is amiss...");
            goto raise_dead;
        }
        return;
    }

    /* when not an invocation situation */
    if (invoked) {
        pline("Nothing happens.");
        return;
    }

    if (book2->cursed) {
    raise_dead:

        if (Hallucination)
            You_hear("Michael Jackson dancing!");
        else
            pline("You raised the dead!");
        /* first maybe place a dangerous adversary; don't bother with
           MM_CREATEMONSTER, that's mostly used to ensure that consistent
           species of monsters generate */
        if (!rn2(3) &&
            ((mtmp = makemon(&mons[PM_MASTER_LICH], level, u.ux, u.uy,
                             NO_MINVENT)) != 0 ||
             (mtmp = makemon(&mons[PM_NALFESHNEE], level, u.ux, u.uy,
                             NO_MINVENT)) != 0)) {
            msethostility(mtmp, TRUE, TRUE);
        }
        /* next handle the effect on things you're carrying */
        unturn_dead(&youmonst);
        /* last place some monsters around you */
        mm.x = u.ux;
        mm.y = u.uy;
        mkundead(level, &mm, TRUE, NO_MINVENT);
    } else if (book2->blessed) {
        for (mtmp = level->monlist; mtmp; mtmp = mtmp2) {
            mtmp2 = mtmp->nmon; /* tamedog() changes chain */
            if (DEADMONSTER(mtmp))
                continue;

            if (is_undead(mtmp->data) && cansee(mtmp->mx, mtmp->my)) {
                msethostility(mtmp, FALSE, FALSE); /* TODO: reset alignment? */
                if (sgn(mtmp->data->maligntyp) == sgn(u.ualign.type)
                    && distu(mtmp->mx, mtmp->my) < 4)
                    if (mtmp->mtame) {
                        if (mtmp->mtame < 20)
                            mtmp->mtame++;
                    } else
                        tamedog(mtmp, NULL);
                else
                    monflee(mtmp, 0, FALSE, TRUE);
            }
        }
    } else {
        switch (rn2(3)) {
        case 0:
            pline("Your ancestors are annoyed with you!");
            break;
        case 1:
            pline("The headstones in the cemetery begin to move!");
            break;
        default:
            pline("Oh my!  Your name appears in the book!");
        }
    }
    return;
}

int RHS_centered ( H_DBL t, H_DBL *x, H_DBL *u, H_DBL *f,
                   int i, int N, void *vparams )
{
  H_DBL h = *(H_DBL *) vparams;
  H_DBL eps = 0.02/h;
  
  t=x[0];
  t=u[0];
  t=h;
  /* VL(("called RHS_centered\n")); */

  f[abs(i)] = u[N+abs(i)];

  if ( i >=3 && i<= N-3 )
      f[N+abs(i)] = fda_D2_5_inner_node ( u, h, i ) + 0.0/x[abs(i)]*fda_D1_5_inner_node ( u, h, i ) + eps*(u[-3+i]-6*u[-2+i]+15*u[-1+i]-20*u[i]+15*u[1+i]-6*u[2+i]+u[3+i]);
  else if ( i == 2 )
      f[N+abs(i)] = fda_D2_5_inner_node ( u, h, i ) + 0.0/x[abs(i)]*fda_D1_5_inner_node ( u, h, i ) + eps*(2*u[sgn(i)*(-2+i)]-13*u[sgn(i)*(-1+i)]+36*u[sgn(i)*(i)]-55*u[sgn(i)*(1+i)]+50*u[sgn(i)*(2+i)]-27*u[sgn(i)*(3+i)]+8*u[sgn(i)*(4+i)]-u[sgn(i)*(5+i)]);
  else if ( i == N-2 ) {
      i = -i;
      f[N+abs(i)] = fda_D2_5_inner_node ( u, h, i ) + 0.0/x[abs(i)]*fda_D1_5_inner_node ( u, h, i ) + eps*(2*u[sgn(i)*(-2+i)]-13*u[sgn(i)*(-1+i)]+36*u[sgn(i)*(i)]-55*u[sgn(i)*(1+i)]+50*u[sgn(i)*(2+i)]-27*u[sgn(i)*(3+i)]+8*u[sgn(i)*(4+i)]-u[sgn(i)*(5+i)]);
  }
  else
      f[N+abs(i)] = fda_D2_5_inner_node ( u, h, i ) + 0.0/x[abs(i)]*fda_D1_5_inner_node ( u, h, i );

    /* f[N+abs(i)] = fda_D2_5_inner_node ( u, h, i ) + 2.0/x[i]*fda_D1_5_inner_node ( u, h, i ); */
    /* f[N+abs(i)] = fda_D2_3_inner_node( u, h, abs(i) ) + 2.0/x[abs(i)]*fda_D1_3_inner_node ( u, h, abs(i) ); */

  return H_TRUE;
}

 Matrix4cd gamma(const int index)
 {
   int prefactor = sgn(index);
   return prefactor * gammas[abs(index) - 1];
 }

double Problem2Backward2D::penalty(unsigned int i, const TimeNodePDE &tn) const
{
    return func->gpi(i, tn.i, *info)*sgn(func->g0i(i, tn.i, *info));
}

//------------------------------------------------------------------------
void CScriptControlledPhysics::OnPostStep(EventPhysPostStep *pPostStep)
{
    pe_action_set_velocity av;

    bool moving=m_moving;
    bool rotating=m_rotating;

    float dt=pPostStep->dt;

    if (m_moving)
    {
        Vec3 current=pPostStep->pos;
        Vec3 target=m_moveTarget;
        Vec3 delta=target-current;
        float distance=delta.len();
        Vec3 dir=delta;
        if(distance>0.01f)
            dir *= (1.0f/distance);

        if (distance<0.01f)
        {
            m_speed=0.0f;
            m_moving=false;

            pPostStep->pos=target;

            av.v=ZERO;
        }
        else
        {
            float a=m_speed/m_stopTime;
            float d=m_speed*m_stopTime-0.5f*a*m_stopTime*m_stopTime;

            if (distance<=(d+0.01f))
                m_acceleration=(distance-m_speed*m_stopTime)/(m_stopTime*m_stopTime);

            m_speed=m_speed+m_acceleration*dt;
            if (m_speed>=m_maxSpeed)
            {
                m_speed=m_maxSpeed;
                m_acceleration=0.0f;
            }
            else if (m_speed*dt>distance)
                m_speed=distance/dt;
            else if (m_speed<0.05f)
                m_speed=0.05f;

            av.v=dir*m_speed;
        }
    }

    if (m_rotating)
    {
        Quat current=pPostStep->q;
        Quat target=m_rotationTarget;

        Quat rotation=target*!current;
        float angle=cry_acosf(CLAMP(rotation.w, -1.0f, 1.0f))*2.0f;
        float original=angle;
        if (angle>gf_PI)
            angle=angle-gf_PI2;
        else if (angle<-gf_PI)
            angle=angle+gf