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