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 { Vec3 pos = pPart->GetWorldTM().GetTranslation(); AABB aabb = pPart->GetLocalBounds(); float radius = aabb.GetRadius(); Vec3 vRadius(radius,radius,radius); AABB areaBox(pos-vRadius, pos+vRadius); IRenderNode * pRenderNode = NULL; if (IEntityRenderProxy *pRenderProxy = (IEntityRenderProxy*)pEntity->GetProxy(ENTITY_PROXY_RENDER)) pRenderNode = pRenderProxy->GetRenderNode(); gEnv->p3DEngine->DeleteDecalsInRange(&areaBox, pRenderNode); } } } else { if (pWheel) { // restore wheel properties IPhysicalEntity* pPhysics = m_pVehicle->GetEntity()->GetPhysics(); pe_params_wheel wheelParams; for (int i=0; i<m_pVehicle->GetWheelCount(); ++i) { const pe_cargeomparams* pParams = m_pVehicle->GetWheelPart(i)->GetIWheel()->GetCarGeomParams(); wheelParams.iWheel = i; wheelParams.bBlocked = 0; wheelParams.suspLenMax = pParams->lenMax; wheelParams.bDriving = pParams->bDriving; wheelParams.minFriction = pParams->minFriction; wheelParams.maxFriction = pParams->maxFriction; pPhysics->SetParams(&wheelParams); } if (IVehicleMovement* pMovement = m_pVehicle->GetMovement()) { SVehicleMovementEventParams params; params.pComponent = pComponent; params.iValue = pWheel->GetWheelIndex(); // reset the particle status pMovement->OnEvent(IVehicleMovement::eVME_TireRestored, params); } } m_aiImmobilizedTimer = -1; } m_isActive = activate; }
//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); rotspeed = HTGYROreadRot(gyro); //Read the current rotation speed rcur += rotspeed * time1[T2]*0.001; //Magic nxtDisplayCenteredBigTextLine(3, "%2.0f", rcur); } motor[fr] = 0; motor[br] = 0; motor[bl] = 0; motor[fl] = 0; 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); wait1Msec(1000); } motor[fr] = spd*(-_x1*_x1s+_y1*_y1s-_x2); motor[br] = spd*(_x1*_x1s+_y1*_y1s-_x2); motor[bl] = spd*(_x1*_x1s-_y1*_y1s-_x2); motor[fl] = spd*(-_x1*_x1s-_y1*_y1s-_x2); atgoal = !((dirx == 1 && xgoal > xcur) || (dirx == -1 && xgoal < xcur) || (diry == 1 && ygoal > ycur) || (diry == -1 && ygoal < ycur) || round(rcur) != rgoal); time1[T1] = 0; //Reset timer } motor[fr] = 0; motor[br] = 0; motor[bl] = 0; motor[fl] = 0; } }
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 { //if ( activeShootTimer.time()<0.3 ) //{ // shoot_pow = 1; //} } //if (attackFuckingAngle()) { // shootAngle = AngleWith(ball.Position, Vec2(side*field_width, 0)); // shoot_pow = 1; //} debugDraw = true; tech_circle(attack, shootAngle, shoot_pow, 0, 1, 0, 0, 0); //circle_ball(attack, 90, 80, 0, 1.0f); debugDraw = false; } } } if (ball.Position.Y>600) { recievePass(mid1, Vec2 ( -side*250 , 0 )); } else { recievePass(mid1, Vec2 ( -side*(field_width-800) , field_height-800 )); } if (ball.Position.Y<-600) { recievePass(mid2, Vec2 ( -side*250 , 0 )); } else { recievePass(mid2, Vec2 ( -side*(field_width-800) ,-field_height+800 )); } }
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); DEBUG_CONTROLLER_LOG_BUTTON_ACTION(pSymbol); } } } // now we have done the digital buttons ... let's do the analog stuff if (m_state.Gamepad.bLeftTrigger != state.Gamepad.bLeftTrigger) { pSymbol = DevSpecIdToSymbol(_XINPUT_GAMEPAD_LEFT_TRIGGER); pSymbol->ChangeEvent(state.Gamepad.bLeftTrigger / 255.0f); pSymbol->AssignTo(event); event.deviceType = eIDT_Gamepad; GetIInput().PostInputEvent(event); DEBUG_CONTROLLER_LOG_BUTTON_ACTION(pSymbol); //--- Check previous and current trigger against threshold for digital press/release event bool bIsPressed=state.Gamepad.bLeftTrigger>XINPUT_GAMEPAD_TRIGGER_THRESHOLD ? true : false; bool bWasPressed=m_state.Gamepad.bLeftTrigger>XINPUT_GAMEPAD_TRIGGER_THRESHOLD ? true : false; if(bIsPressed!=bWasPressed) { pSymbol=DevSpecIdToSymbol(_XINPUT_GAMEPAD_LEFT_TRIGGER_BTN); pSymbol->PressEvent(bIsPressed); pSymbol->AssignTo(event); event.deviceType = eIDT_Gamepad; GetIInput().PostInputEvent(event); DEBUG_CONTROLLER_LOG_BUTTON_ACTION(pSymbol); } } if (m_state.Gamepad.bRightTrigger != state.Gamepad.bRightTrigger) { pSymbol = DevSpecIdToSymbol(_XINPUT_GAMEPAD_RIGHT_TRIGGER); pSymbol->ChangeEvent(state.Gamepad.bRightTrigger / 255.0f); pSymbol->AssignTo(event); event.deviceType = eIDT_Gamepad; GetIInput().PostInputEvent(event); DEBUG_CONTROLLER_LOG_BUTTON_ACTION(pSymbol); //--- Check previous and current trigger against threshold for digital press/release event bool bIsPressed=state.Gamepad.bRightTrigger>XINPUT_GAMEPAD_TRIGGER_THRESHOLD; bool bWasPressed=m_state.Gamepad.bRightTrigger>XINPUT_GAMEPAD_TRIGGER_THRESHOLD; if(bIsPressed!=bWasPressed) { pSymbol=DevSpecIdToSymbol(_XINPUT_GAMEPAD_RIGHT_TRIGGER_BTN); pSymbol->PressEvent(bIsPressed); pSymbol->AssignTo(event); event.deviceType = eIDT_Gamepad; GetIInput().PostInputEvent(event); DEBUG_CONTROLLER_LOG_BUTTON_ACTION(pSymbol); } } if ((m_state.Gamepad.sThumbLX != state.Gamepad.sThumbLX) || m_forceResendSticks) { pSymbol = DevSpecIdToSymbol(_XINPUT_GAMEPAD_LEFT_THUMB_X); if(state.Gamepad.sThumbLX==0.f) pSymbol->ChangeEvent(0.f); else pSymbol->ChangeEvent((state.Gamepad.sThumbLX + 32768) / 32767.5f - 1.0f); pSymbol->AssignTo(event); event.deviceType = eIDT_Gamepad; GetIInput().PostInputEvent(event); //--- Check previous and current state to generate digital press/release event static SInputSymbol* pSymbolLeft = DevSpecIdToSymbol(_XINPUT_GAMEPAD_LEFT_THUMB_LEFT); ProcessAnalogStick(pSymbolLeft, m_state.Gamepad.sThumbLX, state.Gamepad.sThumbLX, -25000); static SInputSymbol* pSymbolRight = DevSpecIdToSymbol(_XINPUT_GAMEPAD_LEFT_THUMB_RIGHT); ProcessAnalogStick(pSymbolRight, m_state.Gamepad.sThumbLX, state.Gamepad.sThumbLX, 25000); } if ((m_state.Gamepad.sThumbLY != state.Gamepad.sThumbLY) || m_forceResendSticks) { pSymbol = DevSpecIdToSymbol(_XINPUT_GAMEPAD_LEFT_THUMB_Y); if(state.Gamepad.sThumbLY==0.f) pSymbol->ChangeEvent(0.f); else pSymbol->ChangeEvent((state.Gamepad.sThumbLY + 32768) / 32767.5f - 1.0f); pSymbol->AssignTo(event); event.deviceType = eIDT_Gamepad; GetIInput().PostInputEvent(event); //--- Check previous and current state to generate digital press/release event static SInputSymbol* pSymbolUp = DevSpecIdToSymbol(_XINPUT_GAMEPAD_LEFT_THUMB_UP); ProcessAnalogStick(pSymbolUp, m_state.Gamepad.sThumbLY, state.Gamepad.sThumbLY, 25000); static SInputSymbol* pSymbolDown = DevSpecIdToSymbol(_XINPUT_GAMEPAD_LEFT_THUMB_DOWN); ProcessAnalogStick(pSymbolDown, m_state.Gamepad.sThumbLY, state.Gamepad.sThumbLY, -25000); } if ((m_state.Gamepad.sThumbRX != state.Gamepad.sThumbRX) || m_forceResendSticks) { pSymbol = DevSpecIdToSymbol(_XINPUT_GAMEPAD_RIGHT_THUMB_X); if(state.Gamepad.sThumbRX==0.f) pSymbol->ChangeEvent(0.f); else pSymbol->ChangeEvent((state.Gamepad.sThumbRX + 32768) / 32767.5f - 1.0f); pSymbol->AssignTo(event); event.deviceType = eIDT_Gamepad; GetIInput().PostInputEvent(event); //--- Check previous and current state to generate digital press/release event static SInputSymbol* pSymbolLeft = DevSpecIdToSymbol(_XINPUT_GAMEPAD_RIGHT_THUMB_LEFT); ProcessAnalogStick(pSymbolLeft, m_state.Gamepad.sThumbRX, state.Gamepad.sThumbRX, -25000); static SInputSymbol* pSymbolRight = DevSpecIdToSymbol(_XINPUT_GAMEPAD_RIGHT_THUMB_RIGHT); ProcessAnalogStick(pSymbolRight, m_state.Gamepad.sThumbRX, state.Gamepad.sThumbRX, 25000); } if ((m_state.Gamepad.sThumbRY != state.Gamepad.sThumbRY) || m_forceResendSticks) { pSymbol = DevSpecIdToSymbol(_XINPUT_GAMEPAD_RIGHT_THUMB_Y); if(state.Gamepad.sThumbRY==0.f) pSymbol->ChangeEvent(0.f); else pSymbol->ChangeEvent((state.Gamepad.sThumbRY + 32768) / 32767.5f - 1.0f); pSymbol->AssignTo(event); event.deviceType = eIDT_Gamepad; GetIInput().PostInputEvent(event); //--- Check previous and current state to generate digital press/release event static SInputSymbol* pSymbolUp = DevSpecIdToSymbol(_XINPUT_GAMEPAD_RIGHT_THUMB_UP); ProcessAnalogStick(pSymbolUp, m_state.Gamepad.sThumbRY, state.Gamepad.sThumbRY, 25000); static SInputSymbol* pSymbolDown = DevSpecIdToSymbol(_XINPUT_GAMEPAD_RIGHT_THUMB_DOWN); ProcessAnalogStick(pSymbolDown, m_state.Gamepad.sThumbRY, state.Gamepad.sThumbRY, -25000); } // update cache m_state = state; m_forceResendSticks = false; } } }
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) { m_rotationSpeed=sgn(m_rotationSpeed)*m_rotationMaxSpeed; m_rotationAcceleration=0.0f; } } if(cry_fabsf(angle)>=0.001f) av.w=(rotation.v/cry_sinf(original*0.5f)).normalized(); av.w*=m_rotationSpeed; } if (moving || rotating) { if (IPhysicalEntity *pPE=GetEntity()->GetPhysics()) pPE->Action(&av, 1); } /* if ((moving && !m_moving) || (rotating && !m_rotating)) GetEntity()->SetWorldTM(Matrix34::Create(GetEntity()->GetScale(), pPostStep->q, pPostStep->pos)); */ }
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); } }
/* special effects for The Book of the Dead */ void deadbook(struct obj *book2, boolean invoked) { struct monst *mtmp, *mtmp2; coord mm; if (!invoked) pline("You turn the pages of the Book of the Dead..."); makeknown(SPE_BOOK_OF_THE_DEAD); /* KMH -- Need ->known to avoid "_a_ Book of the Dead" */ book2->known = 1; if (invocation_pos(&u.uz, u.ux, u.uy) && !On_stairs(u.ux, u.uy)) { struct obj *otmp; boolean arti1_primed = FALSE, arti2_primed = FALSE, arti_cursed = FALSE; if (invoked) { if (Blind) You_hear("a crisp flicker..."); else pline("The Book of the Dead opens of its own accord..."); } if (book2->cursed) { if (invoked) { if (Hallucination) You_hear("gratuitous bleeping."); else You_hear("a mumbled curse."); } else pline("The runes appear scrambled. You can't read them!"); return; } if (!Uhave_bell || !Uhave_menorah) { pline("A chill runs down your %s.", body_part(SPINE)); if (!Uhave_bell) { if (Hallucination) pline("You feel like a tuning fork!"); else You_hear("a faint chime..."); } if (!Uhave_menorah) { if (Hallucination) { pline("Nosferatu giggles."); } else if (mvitals[PM_DOPPELGANGER].mvflags & G_GENOD) { /* suggestion by b_jonas: can't talk about doppelgangers if they don't exist */ if (Uhave_bell) pline("Nothing seems to happen."); /* otherwise no message, we already printed one. */ } else { pline("Vlad's doppelganger is amused."); } } return; } for (otmp = invent; otmp; otmp = otmp->nobj) { if (otmp->otyp == CANDELABRUM_OF_INVOCATION && otmp->spe == 7 && otmp->lamplit) { if (!otmp->cursed) 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_PI2; if (cry_fabsf(angle)<0.001f) { 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*dt; if (cry_fabsf(m_rotationSpeed*dt)>cry_fabsf(angle)) m_rotationSpeed=angle/dt; else if (cry_fabsf(m_rotationSpeed)<0.001f) m_rotationSpeed=sgn(m_rotationSpeed)*0.001f; else if (cry_fabsf(m_rotationSpeed)>=m_rotationMaxSpeed) { m_rotationSpeed=sgn(m_rotationSpeed)*m_rotationMaxSpeed; m_rotationAcceleration=0.0f; } } if(cry_fabsf(angle)>=0.001f) av.w=(rotation.v/cry_sinf(original*0.5f)).normalized(); av.w*=m_rotationSpeed; } if (moving || rotating) { if (IPhysicalEntity *pPE=GetEntity()->GetPhysics()) pPE->Action(&av); } if ((moving && !m_moving) || (rotating && !m_rotating)) GetEntity()->SetWorldTM(Matrix34::Create(GetEntity()->GetScale(), pPostStep->q, pPostStep->pos)); }
//------------------------------------------------------------------------ void CVehiclePartTread::Update(const float frameTime) { FUNCTION_PROFILER( GetISystem(), PROFILE_ACTION ); if (!m_pCharInstance) return; if (m_bForceSetU) { m_bForceSetU = false; m_currentU = m_wantedU + 1.0f; UpdateU(); } ISkeletonPose* pSkeletonPose = m_pCharInstance->GetISkeletonPose(); pSkeletonPose->SetForceSkeletonUpdate(0); if (VehicleCVars().v_staticTreadDeform == 0 && m_pVehicle->GetStatus().speed < 0.001f) { return; } if (frameTime > 0.f && (m_damageRatio >= 1.f || !m_pVehicle->GetGameObject()->IsProbablyVisible() || m_pVehicle->IsProbablyDistant())) { return; } // we need a tread update in next frame pSkeletonPose->SetForceSkeletonUpdate(1); m_pVehicle->NeedsUpdate(); // animate the UV texture according to the wheels speed if (m_uvSpeedMultiplier != 0.0f && frameTime > 0.0f) { IPhysicalEntity* pPhysics = GetEntity()->GetPhysics(); pe_status_wheel wheelStatus; wheelStatus.iWheel = m_lastWheelIndex; if (pPhysics && pPhysics->GetStatus(&wheelStatus) != 0) { m_wantedU += m_uvSpeedMultiplier * (wheelStatus.w * wheelStatus.r * frameTime); m_wantedU -= std::floor(m_wantedU); UpdateU(); } } // deform the tread to follow the wheels QuatT absRoot = pSkeletonPose->GetAbsJointByID(0); for (TWheelInfoVector::const_iterator ite=m_wheels.begin(), end=m_wheels.end(); ite != end; ++ite) { const SWheelInfo& wheelInfo = *ite; const Matrix34& slotTM = GetEntity()->GetSlotLocalTM(wheelInfo.slot, true); VALIDATE_MAT(slotTM); if (m_operatorQueue) { m_operatorQueue->PushPosition(wheelInfo.jointId, IAnimationOperatorQueue::eOp_Override, slotTM.GetTranslation()); } #if ENABLE_VEHICLE_DEBUG if (VehicleCVars().v_debugdraw == 4) { Vec3 local = GetEntity()->GetWorldTM().GetInverted() * slotTM.GetTranslation(); gEnv->pRenderer->GetIRenderAuxGeom()->DrawSphere(GetEntity()->GetWorldTM() * (local+Vec3((float)sgn(local.x)*0.5f,0.f,0.f)),0.1f,ColorB(0,0,255,255)); } #endif } ISkeletonAnim* pSkeletonAnim = m_pCharInstance->GetISkeletonAnim(); pSkeletonAnim->PushPoseModifier(VEH_ANIM_POSE_MODIFIER_LAYER, m_operatorQueue, "VehiclePartAnimatedJoint"); }
void ai09::circle_ball ( int robot_num , float tagret_angle , int shoot_pow , int chip_pow , float precision , float near_dis_override ) { //tagret_angle -= 5; const float very_far_ball_dis = 600.0f; const float far_ball_dis = 160.0f; const int far_to_near_hys = 5; float near_ball_dis = 140.0f; if ( near_dis_override > 0 ) near_ball_dis = near_dis_override; const float near_angle_tol = 4.0f; const int near_to_kick_hys = 3; const float shmit_coeff = 1.2f; static ball_circling_state state = very_far; static float last_change_t = 0.0f; static int hys_bank[4]={0,0,0,0}; if (timer.time()<0.1) { state = very_far; last_change_t = timer.time(); hys_bank[0]=hys_bank[1]=hys_bank[2]=hys_bank[3]=0; Halt(robot_num); return; } if (state == very_far) { OwnRobot[robot_num].face(ball.Position); ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1); ERRTNavigate2Point(robot_num, ball.Position, 1, 30, &VELOCITY_PROFILE_AROOM); AddDebugCircle(ball.Position,very_far_ball_dis-90.0f,Red); if (DIS(OwnRobot[robot_num].State.Position, ball.Position) < very_far_ball_dis) { state = far; last_change_t = timer.time(); } } else if (state == far) { OwnRobot[robot_num].face(ball.Position); ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1); TVec2 target_point = CircleAroundPoint(ball.Position, AngleWith(ball.Position, OwnRobot[robot_num].State.Position), near_ball_dis); ERRTNavigate2Point(robot_num, target_point, 1, 20, &VELOCITY_PROFILE_AROOM); AddDebugCircle(ball.Position,far_ball_dis-90.0f,Pink); if (DIS(OwnRobot[robot_num].State.Position, ball.Position) < far_ball_dis) { hys_bank[0]++; } else { hys_bank[0]=0; } if (hys_bank[0] > far_to_near_hys ) { state = near; last_change_t = timer.time(); } else if (DIS(OwnRobot[robot_num].State.Position, ball.Position) > very_far_ball_dis * shmit_coeff) { state = very_far; last_change_t = timer.time(); } } else if (state == near) { float toRobot = AngleWith(ball.Position, OwnRobot[robot_num].State.Position); float newToRobot = NormalizeAngle(toRobot-tagret_angle); float deltaAngle = min(fabs(newToRobot), 30.0f); newToRobot = max(0.0f,fabs(newToRobot)-deltaAngle)*sgn(newToRobot); newToRobot = NormalizeAngle(newToRobot+tagret_angle); OwnRobot[robot_num].face(ball.Position); OwnRobot[robot_num].target.Angle += NormalizeAngle(newToRobot+180.0f-OwnRobot[robot_num].target.Angle)/2.0f; //OwnRobot[robot_num].target.Angle = NormalizeAngle(OwnRobot[robot_num].target.Angle); ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1); TVec2 target_point = CircleAroundPoint(ball.Position, newToRobot, near_ball_dis/cosDeg(deltaAngle)); if ( near_dis_override > 0 ) ERRTNavigate2Point(robot_num, target_point, 1, 20, &VELOCITY_PROFILE_AROOM); else ERRTNavigate2Point(robot_num, target_point, 1, 20, &VELOCITY_PROFILE_MAMOOLI); if (DIS(OwnRobot[robot_num].State.Position, ball.Position) > far_ball_dis*shmit_coeff) { state = far; last_change_t = timer.time(); } if (fabs(deltaAngle) < near_angle_tol) { hys_bank[0]++; } else { hys_bank[0]=0; } if ((hys_bank[0]>near_to_kick_hys)&&((shoot_pow>0)||(chip_pow>0))) { state = kick; last_change_t = timer.time(); } } else if (state == kick) { if (chip_pow>0) { chip_head = OwnRobot[robot_num].State.Angle; } //OwnRobot[robot_num].face(ball.Position); OwnRobot[robot_num].target.Angle=NormalizeAngle(tagret_angle+180.0f); ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1); ERRTNavigate2Point(robot_num, ball.Position, 1, 100, &VELOCITY_PROFILE_AROOM); if ( shoot_pow > 0 ) OwnRobot[robot_num].Shoot(shoot_pow); if ( chip_pow > 0 ) OwnRobot[robot_num].Chip(chip_pow); AddDebugCircle(ball.Position,very_far_ball_dis-90.0f,Red); //tech_circle(robot_num, tagret_angle, shoot_pow, chip_pow, 1, 1, 0, 0); if (DIS(OwnRobot[robot_num].State.Position, ball.Position) > near_ball_dis*shmit_coeff) { state = far; last_change_t = timer.time(); } } AddDebugLine(OwnRobot[robot_num].State.Position,OwnRobot[robot_num].target.Position,Black); }
// Calculates Output from Input based on a Linear Relationship int AttenuateControllerOutput(int output) { // using linear equation in slope-intercept form of y = mx + b return (LINEAR_MOTOR_CONTROL_SLOPE*output + sgn(output)*LINEAR_MOTOR_CONTROL_INTERCEPT); }
int16_t PidControllerUpdate( pidController *p ) { if( p == NULL ) return(0); if( p->enabled ) { // check for sensor port // otherwise externally calculated error if( p->sensor_port >= 0 ) { // Get raw position value, may be pot or encoder p->sensor_value = vexSensorValueGet( p->sensor_port ); // A reversed sensor ? if( p->sensor_reverse ) { if( vexSensorIsAnalog( p->sensor_port) ) // reverse pot p->sensor_value = 4095 - p->sensor_value; else // reverse encoder p->sensor_value = -p->sensor_value; } p->error = p->target_value - p->sensor_value; } // force error to 0 if below threshold if( fabs(p->error) < p->error_threshold ) p->error = 0; // integral accumulation if( p->Ki != 0 ) { p->integral += p->error; // limit to avoid windup if( fabs( p->integral ) > p->integral_limit ) p->integral = sgn(p->integral) * p->integral_limit; } else p->integral = 0; // derivative p->derivative = p->error - p->last_error; p->last_error = p->error; // calculate drive - no delta T in this version p->drive = (p->Kp * p->error) + (p->Ki * p->integral) + (p->Kd * p->derivative) + p->Kbias; // drive should be in the range +/- 1.0 if( fabs( p->drive ) > 1.0 ) p->drive = sgn(p->drive); // final motor output p->drive_raw = p->drive * 127.0; } else { // Disabled - all 0 p->error = 0; p->last_error = 0; p->integral = 0; p->derivative = 0; p->drive = 0.0; p->drive_raw = 0; } // linearize - be careful this is a macro p->drive_cmd = _LinearizeDrive( p->drive_raw ); // return the thing we are really interested in return( p->drive_cmd ); }
void FastMarchingMethod :: updateTrialValue(FloatArray &dmanValues, int id, double F) { int ai, bi, ci, h, nroot, _ind = 0; double at, bt, ht, a, b, u, cos_fi, sin_fi, _a, _b, _c, r1, r2, r3, t = 0.0, _h; bool reg_upd_flag; FloatArray *ac, *bc, *cc, cb, ca; ConnectivityTable *ct = domain->giveConnectivityTable(); // first look for suitable element that can produce admissible value // algorithm limited to non-obtuse 2d triangulations for ( int neighborElem: *ct->giveDofManConnectivityArray(id) ) { // test element if admissible Element *ie = domain->giveElement( neighborElem ); if ( ie->giveGeometryType() == EGT_triangle_1 ) { for ( int j = 1; j <= 3; j++ ) { if ( ie->giveDofManagerNumber(j) == id ) { _ind = j; } } ci = ie->giveDofManagerNumber(_ind); ai = ie->giveDofManagerNumber(1 + ( _ind ) % 3); bi = ie->giveDofManagerNumber(1 + ( _ind + 1 ) % 3); if ( ( dmanRecords.at(ai - 1).status == FMM_Status_KNOWN ) && ( dmanRecords.at(bi - 1).status == FMM_Status_KNOWN ) ) { at = dmanValues.at(ai); bt = dmanValues.at(bi); if ( fabs(at) > fabs(bt) ) { h = ai; ai = bi; bi = h; ht = at; at = bt; bt = ht; } // get nodal coordinates ac = domain->giveNode(ai)->giveCoordinates(); bc = domain->giveNode(bi)->giveCoordinates(); cc = domain->giveNode(ci)->giveCoordinates(); // a = distance of BC a = cc->distance(bc); // b = distance of AC b = cc->distance(ac); // compute fi angle cb.beDifferenceOf(* bc, * cc); cb.normalize(); ca.beDifferenceOf(* ac, * cc); ca.normalize(); cos_fi = cb.dotProduct(ca); sin_fi = sqrt(1.0 - cos_fi * cos_fi); u = fabs(bt); // compute quadratic equation coefficients for t _a = ( a * a + b * b - 2.0 * a * b * cos_fi ); _b = 2.0 * b * u * ( a * cos_fi - b ); _c = b * b * ( u * u - F * F * a * a * sin_fi * sin_fi ); cubic3r(0.0, _a, _b, _c, & r1, & r2, & r3, & nroot); reg_upd_flag = true; if ( nroot == 0 ) { reg_upd_flag = false; } else if ( nroot == 1 ) { t = r1; } else if ( r1 >= 0.0 ) { t = r1; } else if ( r2 >= 0.0 ) { t = r2; } else { reg_upd_flag = false; } if ( reg_upd_flag ) { _h = b * ( t - u ) / t; if ( ( t > u ) && ( _h > a * cos_fi ) && ( _h < a / cos_fi ) ) { if ( dmanRecords.at(ci - 1).status == FMM_Status_FAR ) { dmanValues.at(ci) = sgn(F) * t + at; } else if ( F > 0. ) { dmanValues.at(ci) = min(dmanValues.at(ci), sgn(F) * t + at); } else { dmanValues.at(ci) = max(dmanValues.at(ci), sgn(F) * t + at); } } else { reg_upd_flag = false; } } if ( !reg_upd_flag ) { if ( F > 0. ) { _h = min(b * F + at, a * F + bt); } else { _h = max(b * F + at, a * F + bt); } if ( dmanRecords.at(ci - 1).status == FMM_Status_FAR ) { dmanValues.at(ci) = _h; } else if ( F > 0. ) { dmanValues.at(ci) = min(dmanValues.at(ci), _h); } else { dmanValues.at(ci) = max(dmanValues.at(ci), _h); } } // if not yet in queue (trial for the first time), put it there if ( dmanRecords.at(ci - 1).status != FMM_Status_TRIAL ) { dmanTrialQueue.push(ci); } dmanRecords.at(ci - 1).status = FMM_Status_TRIAL; } // admissible triangle } // end EGT_triangle_1 element type } }
//This function assumes that the robot only needs to move left and right to find the beacon: //Make sure x,y,r is a correct starting position void seekIR(){ int _dirEnh; int _strEnh; float dir; float time = 0; HTIRS2readEnhanced(IR, _dirEnh, _strEnh); time1[T1] = 0; while (_dirEnh != 6) { time1[T1] = 0; HTIRS2readEnhanced(IR, _dirEnh, _strEnh); if (_dirEnh == 0) { motor[fr] = 0; motor[br] = 0; motor[bl] = 0; motor[fl] = 0; PlaySound(soundShortBlip); } else if (_dirEnh < 6){ motor[fr] = 30; motor[br] = -30; motor[bl] = -30; motor[fl] = 30; dir = -1; } else if (_dirEnh > 6) { motor[fr] = -30; motor[br] = 30; motor[bl] = 30; motor[fl] = -30; dir = 1; } time += dir*time1[T1]; } //Edit this according to basket position motor[fr] = -30; //Sideways motor[br] = 30; motor[bl] = 30; motor[fl] = -30; wait1Msec(500); motor[fr] = -30; //Backwards motor[br] = -30; motor[bl] = 30; motor[fl] = 30; wait1Msec(1200); motor[fr] = 0; //Arm up motor[br] = 0; motor[bl] = 0; motor[fl] = 0; PlaySound(soundBeepBeep); servo[arml] = ARMUPL; servo[armr] = ARMUPR; wait1Msec(1000); motor[fr] = 0; //Arm down motor[br] = 0; motor[bl] = 0; motor[fl] = 0; motor[fr] = 30; //Forward motor[br] = 30; motor[bl] = -30; motor[fl] = -30; wait1Msec(1200); motor[fr] = 0; //Dispose motor[br] = 0; motor[bl] = 0; motor[fl] = 0; motor[jaw] = 40; wait1Msec(1500); motor[jaw] = 0; motor[fr] = -30; //Backward motor[br] = -30; motor[bl] = 30; motor[fl] = 30; wait1Msec(1200); motor[fr] = 0; //Arm down motor[br] = 0; motor[bl] = 0; motor[fl] = 0; PlaySound(soundBeepBeep); servo[arml] = ARMDOWNL; servo[armr] = ARMDOWNR; wait1Msec(1000); motor[fr] = 30; //Forward motor[br] = 30; motor[bl] = -30; motor[fl] = -30; wait1Msec(1200); motor[fr] = 30; //Sideways motor[br] = -30; motor[bl] = -30; motor[fl] = 30; wait1Msec(500); dir = sgn(time); motor[fr] = dir*30; //Back to coordinates motor[br] = dir*-30; motor[bl] = dir*-30; motor[fl] = dir*30; wait1Msec(time); motor[fr] = 0; motor[br] = 0; motor[bl] = 0; motor[fl] = 0; }
task Arm() { int armPosition; // Since the PID is set for speed control, the motor value // is speed, not power. int armSpeed = 0; float delta = 0; float diff = 0; const float MHP = 50.0; //Max Hold Power const float DEAD_BAND = 5.0; // Deadband for hold tolerance int holdPower = 0; float encoderCount; nMotorEncoder[Arm_motor] = 0; nMotorPIDSpeedCtrl[Arm_motor] = mtrSpeedReg; nMaxRegulatedSpeed12V = 1000; //int servoPosition = 0; //bool servoForward = true; // bool clawOpen = true; while (true) { //---------------------------------------------- // If the Joystick is beyond the deadband area, // move the arm based on the position of the // joystick. //---------------------------------------------- while(abs(joystick.joy1_y2) > deadband) { // Set the speed based on the joystick position armSpeed = ((float)joystick.joy1_y2 / 127.0) * 15; motor[Arm_motor] = armSpeed; } // End if the arm is beyond the deadband // Stop the motor and reset the encoder to zero // for a good reference point. (keep it from wrapping motor(Arm_motor) = 0; nMotorEncoder[Arm_motor] = 0; armPosition = 0; //---------------------------------------------- // If the arm moves 5 encoder points from its // position, then move it back. //---------------------------------------------- if(armPosition != nMotorEncoder[Arm_motor]) { delta = armPosition - nMotorEncoder[Arm_motor]; diff = abs( delta ) - DEAD_BAND; if( diff > 0 ) { //holdPower = ( DEAD_BAND / delta ) * MHP; holdPower = ( diff / DEAD_BAND ) * MHP * sgn( delta ); motor[Arm] = holdPower; } } getJoystickSettings(joystick); if(abs(joystick.joy1_y2) > deadband) { motor[Arm] = joystick.joy1_y2;///3; } } }
bool comparex(const Point &a, const Point &b) { return sgn(a.x - b.x) < 0; }
SDL_Color Renderer::raycast(float x, float y, float z, float* ray) { float startX = x; float startY = y; float startZ = z; int size = world->getSize(); // handle x out of bounds if ((x < 0.0f) && (ray[0] > 0.0f)) { float diff = -x; x += diff; // will never be division by zero because of our condition y += (ray[1] * diff) / ray[0]; z += (ray[2] * diff) / ray[0]; } else if ((x > size) && (ray[0] < 0.0f)) { float diff = size - x; x += diff; y += (ray[1] * diff) / ray[0]; z += (ray[2] * diff) / ray[0]; } // handle y out of bounds if ((y < 0.0f) && (ray[1] > 0.0f)) { float diff = -y; y += diff; x += (ray[0] * diff) / ray[1]; z += (ray[2] * diff) / ray[1]; } else if ((y > size) && (ray[1] < 0.0f)) { float diff = size - y; y += diff; x += (ray[0] * diff) / ray[1]; z += (ray[2] * diff) / ray[1]; } // handle z out of bounds if ((z < 0.0f) && (ray[2] > 0.0f)) { float diff = -z; z += diff; x += (ray[0] * diff) / ray[2]; y += (ray[1] * diff) / ray[2]; } else if ((z > size) && (ray[2] < 0.0f)) { float diff = size - z; z += diff; x += (ray[0] * diff) / ray[2]; y += (ray[1] * diff) / ray[2]; } // return skybox if we're still out of bounds if ((x < -0.1f) || (x > size + 0.1f) || (y < -0.1f) || (y > size + 0.1f) || (z < -0.1f) || (z > size + 0.1f)) { return skybox; } float dx = ray[0] * renderDistance; float dy = ray[1] * renderDistance; float dz = ray[2] * renderDistance; float rayMagnitude = sqrt(dx * dx + dy * dy + dz * dz); float ax, ay, az; int sx, sy, sz, n; float sv = numeric_limits<float>::min(); sx = (int) sgn(dx); sy = (int) sgn(dy); sz = (int) sgn(dz); ax = abs(dx) / rayMagnitude; ay = abs(dy) / rayMagnitude; az = abs(dz) / rayMagnitude; ax = ((ax > sv) ? ax : sv); ay = ((ay > sv) ? ay : sv); az = ((az > sv) ? az : sv); float tDeltaX = 1 / ax; float tDeltaY = 1 / ay; float tDeltaZ = 1 / az; float tMaxX = (float) abs((sx == 1) ? (1 - (fmod(x, 1.0f))) : (fmod(x, 1.0f))) / ax; float tMaxY = (float) abs((sy == 1) ? (1 - (fmod(y, 1.0f))) : (fmod(y, 1.0f))) / ay; float tMaxZ = (float) abs((sz == 1) ? (1 - (fmod(z, 1.0f))) : (fmod(z, 1.0f))) / az; n = (int) (abs(dx) + abs(dy) + abs(dz)); int face = -1; while (n-- != 0) { if (tMaxX < tMaxY) { if (tMaxX < tMaxZ) { face = 0; x += sx; tMaxX += tDeltaX; } else { face = 2; z += sz; tMaxZ += tDeltaZ; } } else { if (tMaxY < tMaxZ) { face = 1; y += sy; tMaxY += tDeltaY; } else { face = 2; z += sz; tMaxZ += tDeltaZ; } } Uint8 block = world->getBlock((int) x, (int) y, (int) z); if (block != 0) { SDL_Color c = world->getColor(block); return calculateColor(c, face, startX, x, startY, y, startZ, z); } } return skybox; }
/****************************************************************** * It implements error correction using standard Log_Domain Belief * * Propagation algorithm, described in chapter 11.5.4 of "Inside * * Solid State Drives" book. * ******************************************************************/ word log_bp_error_corrector::correct(channel_data& in) { word c(n); if(in.length() != n){ c.set_err(); return c; } double *W = new double[n]; double **R = new double*[n]; double **M = new double*[n]; for(int i = 0; i < n; i++){ R[i] = new double[n - k](); M[i] = new double[n - k](); /* logarithmic channel likelihood */ W[i] = log(chn.like_ratio(in.get(i))); } /* initialization step */ for(int i = 0; i < n; i++){ std::list<edge> edges = var_nodes[i].get_edges(); std::list<edge>::iterator it; for(it = edges.begin(); it != edges.end(); it++){ int j = (*it).get_dest().get_idx(); R[i][j] = W[i]; } } int idx = 0; bool iscodeword = false; while((idx < imax) && (iscodeword == false)){ /* horizontal step */ for(int j = 0; j < n - k; j++){ std::list<edge> edges = chk_nodes[j].get_edges(); std::list<edge>::iterator it; for(it = edges.begin(); it != edges.end(); it++){ int i = (*it).get_dest().get_idx(); M[i][j] = sgn(R,i,j) * phi(phi(R,i,j)); } } /* vertical step */ for(int i = 0; i < n; i++){ std::list<edge> edges = var_nodes[i].get_edges(); std::list<edge>::iterator it; for(it = edges.begin(); it != edges.end(); it++){ int j = (*it).get_dest().get_idx(); R[i][j] = vertical(M,W[i],i,j); } } /* hard decision and stopping criterion step */ for(int i = 0; i < n; i++){ double l = like_ratio(M,W[i],i); if(l >= 0) c.set(false, i); else c.set(true, i); } /* check if the current word is a codeword */ iscodeword = H.is_codeword(c); idx++; } /* free memory */ for(int i = 0; i < n - k; i++){ delete [] R[i]; delete [] M[i]; } delete [] R; delete [] M; if(iscodeword == false){ c.set_err(); } return c; }
void LineProfileTool::calculateLineProfile(const QPoint& start, const QPoint& end) { QRect rect = d_target->rect(); if (!rect.contains(start) || !rect.contains(end)){ QMessageBox::warning(d_graph, tr("QtiPlot - Pixel selection warning"), tr("Please select the end line point inside the image rectangle!")); return; } QPoint o = d_target->origin(); QPixmap pic = d_target->pixmap(); QImage image = pic.convertToImage(); int x1 = start.x()-o.x(); int x2 = end.x()-o.x(); int y1 = start.y()-o.y(); int y2 = end.y()-o.y(); QSize realSize = pic.size(); QSize actualSize = d_target->size(); if (realSize != actualSize){ double ratioX = (double)realSize.width()/(double)actualSize.width(); double ratioY = (double)realSize.height()/(double)actualSize.height(); x1 = int(x1*ratioX); x2 = int(x2*ratioX); y1 = int(y1*ratioY); y2 = int(y2*ratioY); } QString text = tr("pixel") + "\tx\ty\t" + tr("intensity") + "\n"; //uses the fast Bresenham's line-drawing algorithm #define sgn(x) ((x<0)?-1:((x>0)?1:0)) int i,dx,dy,sdx,sdy,dxabs,dyabs,x,y,px,py,n; dx=x2-x1; //the horizontal distance of the line dy=y2-y1; //the vertical distance of the line dxabs=abs(dx); dyabs=abs(dy); sdx=sgn(dx); sdy=sgn(dy); x=dyabs>>1; y=dxabs>>1; px=x1; py=y1; if (dxabs>=dyabs){ //the line is more horizontal than vertical for(i=0;i<dxabs;i++){ y+=dyabs; if (y>=dxabs){ y-=dxabs; py+=sdy; } px+=sdx; n=dxabs; text+=QString::number(i)+"\t"; text+=QString::number(px)+"\t"; text+=QString::number(py)+"\t"; text+=QString::number(averageImagePixel(image, px, py, true))+"\n"; } } else {// the line is more vertical than horizontal for(i=0;i<dyabs;i++){ x+=dxabs; if (x>=dyabs){ x-=dyabs; px+=sdx; } py+=sdy; n=dyabs; text+=QString::number(i)+"\t"; text+=QString::number(px)+"\t"; text+=QString::number(py)+"\t"; text+=QString::number(averageImagePixel(image, px, py, false))+"\n"; } } Table *t = d_app->newTable(tr("Table") + "1", n, 4, text); MultiLayer* plot = d_app->multilayerPlot(t, QStringList(QString(t->objectName())+"_intensity"), 0); Graph *g = (Graph*)plot->activeGraph(); if (g){ g->setTitle(""); g->setXAxisTitle(tr("pixels")); g->setYAxisTitle(tr("pixel intensity (a.u.)")); } }
void Robot::calculatePosition(int32_t xFieldSize, int32_t yFieldSize, boost::shared_ptr<Robot> secondRobot, int timeDelay) { // tu odpalamy calculateForce i na podstawie wyliczonych wartości // siły oraz bieżących prędkości wyliczamy nowe położenie const double timeStep = static_cast<double>(timeDelay)/1000; //[s] const double robotMass = 1; //[kg] const double maxVelocityX = 0.2*xFieldSize; const double maxVelocityY = 0.2*yFieldSize; const double frictionFactor = 0; const double brakingFactor = 0.3; bool brake = false; Force force = calculateForce(xFieldSize, yFieldSize, secondRobot); /* Siła tłumiąca */ if(force.X > abs(frictionFactor*robotMass*9.81)) force.X += frictionFactor*(-sgn(_xVel))*robotMass*9.81; else brake = true; if(force.Y > abs(frictionFactor*robotMass*9.81)) force.Y += frictionFactor*(-sgn(_yVel))*robotMass*9.81; else brake = true; /* Ograniczenie przemieszczenia zgodnie z maxVelocity */ if((timeStep*_xVel + (force.X/robotMass)*pow(timeStep,2)/2) > maxVelocityX*timeStep) { _xPos = getXPos() + timeStep*maxVelocityX; } else if((timeStep*_xVel + (force.X/robotMass)*pow(timeStep,2)/2) < -maxVelocityX*timeStep) { _xPos = getXPos() - timeStep*maxVelocityX; } else { _xPos = getXPos() + timeStep*_xVel + (force.X/robotMass)*pow(timeStep,2)/2; } if(timeStep*_yVel + (force.Y/robotMass)*pow(timeStep,2)/2 > maxVelocityY*timeStep) { _yPos = getYPos() + timeStep*maxVelocityY; } else if(timeStep*_yVel + (force.Y/robotMass)*pow(timeStep,2)/2 < -maxVelocityY*timeStep) { _yPos = getYPos() - timeStep*maxVelocityY; } else { _yPos = getYPos() + timeStep*_yVel + (force.Y/robotMass)*pow(timeStep,2)/2; } _xVel += (force.X/robotMass)*timeStep; if(brake) _xVel *= brakingFactor; _yVel += (force.Y/robotMass)*timeStep; if(brake) _yVel *= brakingFactor; if(_xVel > maxVelocityX) _xVel = maxVelocityX; else if(_xVel < -maxVelocityX) _xVel = -maxVelocityX; if(_yVel > maxVelocityY) _yVel = maxVelocityY; else if(_yVel < -maxVelocityY) _yVel = -maxVelocityY; }
float CalcCurrent3WIREVamfun( tMotor m, int cmd, int rpm, float v_battery ) { /*-----------------------------------------------------------------------------*/ /* */ /* Estimate current in Vex motor using vamfun's algorithm */ /* subroutine written by Vamfun...Mentor Vex 1508, 599. */ /* 7.13.2012 [email protected]... blog info http://vamfun.wordpress.com */ /* */ /* Modified by James Pearman 7.28.2012 */ /* */ /*-----------------------------------------------------------------------------*/ float v_bemf; float c1, c2; float lamda; float duty_on, duty_off; float i_max, i_bar, i_0; float i_ss_on, i_ss_off; // rescale control value // ports 2 through 9 behave a little differently if( m > port1 && m < port10 ) cmd = (cmd * 128) / 90; //CDS todo.. this needs to be verified for three wire // clip control value to +/- 127 if( abs(cmd) > 127 ) cmd = sgn(cmd) * 127; // which way are we turning ? int dir = (cmd >= 0) ? 1 : (-1); duty_on = abs(cmd)/127.0; // constants for this pwm cycle lamda = R_3WIRE/((float)PWM_FREQ * L_3WIRE); c1 = exp( -lamda * duty_on ); c2 = exp( -lamda * (1-duty_on) ); // Calculate back emf voltage v_bemf = Ke_3WIRE * rpm; // Calculate staady state current for on and off pwm phases i_ss_on = ( v_battery * dir - v_bemf ) / (R_3WIRE + R_SYS); i_ss_off = -( V_DIODE * dir + v_bemf ) / R_3WIRE; // compute trial i_0 i_0 = (i_ss_on*(1-c1)*c2 + i_ss_off*(1-c2))/(1-c1*c2); //check to see if i_0 crosses 0 during off phase if diode were not in circuit if(i_0*dir < 0) { // waveform reaches zero during off phase of pwm cycle hence // ON phase will start at 0 // once waveform reaches 0, diode clamps the current to zero. i_0 = 0; // peak current i_max = i_ss_on*(1-c1); //where does the zero crossing occur duty_off = -log(-i_ss_off/(i_max-i_ss_off))/lamda ; } else { // peak current i_max = i_0*c1 + i_ss_on*(1-c1); // i_0 is non zero so final value of waveform must occur at end of cycle duty_off = 1 - duty_on; } // Average current i_bar = i_ss_on*duty_on + i_ss_off*duty_off; return i_bar; }