void State::Update(double deltaTime) {
if(IsActive() == false) {
ClearImpulses();
ClearForces();
SetVelocity(0.0, 0.0);
SetAcceleration(0.0, 0.0);
return;
}
a2de::Vector2D total_forces;
std::for_each(_forces.begin(), _forces.end(), [&total_forces] (ForceContainer::value_type& current_force) mutable -> a2de::Vector2D
{
if(current_force.second < 0.0) return Vector2D();
total_forces += current_force.first;
return total_forces;
});
Vector2D total_impulses = std::accumulate(_impulses.begin(), _impulses.end(), Vector2D());
Vector2D F(total_forces + total_impulses);
ClearImpulses();
double mass = GetMass();
//If static body, do nothing.
if(Math::IsEqual(mass, 0.0)) {
ClearImpulses();
ClearForces();
SetVelocity(0.0, 0.0);
SetAcceleration(0.0, 0.0);
Sleep();
return;
}
//Integrate from constant acceleration.
//a = F / m
//v = at + v;
//p = (1/2)at^2 + vt + p
SetAcceleration(F / mass);
SetVelocity(GetAcceleration() * deltaTime + GetVelocity());
SetPosition(((0.5 * GetAcceleration()) * deltaTime * deltaTime) + (GetVelocity() * deltaTime) + GetPosition());
_forces.remove_if([=](ForceContainer::value_type& current_force)->bool {
current_force.second -= deltaTime;
return (current_force.second < 0.0);
});
if(_impulses.empty() && _forces.empty() &&
((a2de::Math::IsEqual(_acceleration.GetX(), 0.0) && a2de::Math::IsEqual(_acceleration.GetY(), 0.0)) &&
(a2de::Math::IsEqual(_velocity.GetX(), 0.0) && a2de::Math::IsEqual(_velocity.GetY(), 0.0))))
{
SetVelocity(0.0, 0.0);
SetAcceleration(0.0, 0.0);
Sleep();
} else {
Wake();
}
}
void PhysicsCore::Accelerate(float delta_Time)
{
/******************************************************
* Function Name Accelerate()
* Programmer Alexander Hunsiker
*
* Determines the new velocity by adding the current
* velocity to current acceleration*delta time
******************************************************/
D3DXVECTOR3 max_Velocity = D3DXVECTOR3(5.0f, 5.0f, 5.0f);
D3DXVECTOR3 max_Acceleration = D3DXVECTOR3(-GRAVITY, -GRAVITY, -GRAVITY);
D3DXVECTOR3 cur_Acceleration;
D3DXVECTOR3 cur_Velocity;
D3DXVECTOR3 new_Velocity;
cur_Acceleration = GetAcceleration();
cur_Velocity = GetVelocity();
new_Velocity += cur_Acceleration * delta_Time;
new_Velocity.x *= float(FRICTION);
if(new_Velocity > max_Velocity)
new_Velocity = max_Velocity;
if(new_Velocity < -max_Velocity)
new_Velocity = -max_Velocity;
if(cur_Acceleration > -max_Acceleration)
cur_Acceleration = max_Acceleration;
if(cur_Acceleration < max_Acceleration)
cur_Acceleration = -max_Acceleration;
SetVelocity(new_Velocity);
}
void AttractableSystem::Update(double DeltaTime)
{
for (auto actor : mScene.GetActorsFromMap( GetType_static() ))
{
auto attractableC( actor->Get<IAttractableComponent>() );
auto accelerationC( actor->Get<IAccelerationComponent>() );
auto targetHolderC( actor->Get<ITargetHolderComponent>() );
auto moveC( actor->Get<IMoveComponent>() );
if (!attractableC.IsValid() || !accelerationC.IsValid() || !targetHolderC.IsValid() || !moveC.IsValid())
{
continue;
}
auto const accel = accelerationC->GetAcceleration();
auto currentTarget( mScene.GetActor( targetHolderC->GetTargetGUID() ) );
targetHolderC->SetTargetGUID( -1 );
accelerationC->SetAcceleration( -1 * attractableC->GetDeceleration() );
if (currentTarget.IsValid())
{
auto healthC( currentTarget->Get<IHealthComponent>() );
if (healthC.IsValid() && healthC->IsAlive())
{
accelerationC->SetAcceleration( accel );
targetHolderC->SetTargetGUID( currentTarget->GetGUID() );
moveC->SetMoving( true );
attractableC->GetTurnToTargetAct().Update( *actor, DeltaTime );
}
}
}
}
void Character::UpdateItSelf( float dTime )
{
// 가속
if ( IsAccelerating() )
{
if ( timeGetTime() - GetAccelerationStartTime() > ACCELERATION_TIME )
{
// 가속 끝났다
SetIsAccelerating( false );
SetAcceleration( ZERO_VECTOR3 );
}
}
if ( !m_CharacterClass->IsAlive() )
{
//printf_s( "player %d is dead \n", m_CharacterId );
return;
}
m_Matrix = GetTransform()->MatrixTransform();
if ( IsSpinning() )
{
AddSpinTime( dTime );
// 회전축을 기준으로 물체를 회전시킵니다.
D3DXMATRIXA16 spinTransform;
D3DXVECTOR3 tmpSpinAxis = GetSpinAxis();
float tmpSpinAngle = GetSpinAngularVelocity();
D3DXMatrixRotationAxis( &spinTransform, &tmpSpinAxis, tmpSpinAngle * GetSpinTime() );
// D3DXMatrixMultiply( &m_Matrix, &m_Matrix, &spinTransform );
D3DXMatrixMultiply( &m_Matrix, &spinTransform, &m_Matrix );
}
D3DXVECTOR3 tmpVec3 = GetTransform()->GetPosition();
D3DXVECTOR3 tmpVel = GetVelocity();
Physics::CalcCurrentPosition( &tmpVec3, &tmpVel, GetAcceleration(), dTime, GetSpeedConstant() );
// 최대 이동거리 제한
float currentDistanceFromOrigin = D3DXVec3Length( &tmpVec3 );
if ( currentDistanceFromOrigin > MAX_DISTANCE_FROM_ORIGIN )
m_CharacterClass->SetOxygen( 0.0f );
// 최대 속도 제한
float currentSpeed = D3DXVec3Length( &tmpVel );
if ( currentSpeed > MAX_PLAYER_SPEED )
{
D3DXVec3Normalize( &tmpVel, &tmpVel );
tmpVel *= MAX_PLAYER_SPEED;
}
GetTransform()->SetPosition( tmpVec3 );
SetVelocity( tmpVel );
// 산소량 감소등의 작업 처리
GetClassComponent()->Update( dTime );
}
void RodForceGenerator::Update(double deltaTime) {
if(a2de::Math::IsEqual(_length, 0.0)) return;
if(_rod_ends.first == nullptr) return;
if(_rod_ends.second == nullptr) return;
auto fb = _rod_ends.first->GetComponent<a2de::PhysicsComponent>().body;
auto sb = _rod_ends.second->GetComponent<a2de::PhysicsComponent>().body;
a2de::Vector2D fbp = fb.GetPosition();
a2de::Vector2D sbp = sb.GetPosition();
double square_distance = (fbp - sbp).GetLengthSquared();
double square_length = _length * _length;
if(a2de::Math::IsEqual(square_length, square_distance)) return;
double m1 = fb.GetMass();
double m2 = sb.GetMass();
a2de::Vector2D sb_to_center = (fbp - sbp).Normalize();
a2de::Vector2D fb_to_center = (sbp - fbp).Normalize();
//Update bodies to get up-to-date F=ma calculations for projection.
fb.Update(deltaTime);
sb.Update(deltaTime);
a2de::Vector2D m1force = a2de::Vector2D::GetProjection(m1 * fb.GetAcceleration(), fb_to_center);
a2de::Vector2D m2force = a2de::Vector2D::GetProjection(m2 * sb.GetAcceleration(), sb_to_center);
double distance = std::sqrt(square_distance);
double move_distance = distance - _length;
fb.ApplyImpulse(m1force);
sb.ApplyImpulse(m2force);
double mass_sum = m1 + m2;
double m1_ratio = m1 / mass_sum;
double m2_ratio = m2 / mass_sum;
sb.SetPosition(sbp + ((sb_to_center * move_distance * m2_ratio)));
fb.SetPosition(fbp + ((fb_to_center * move_distance * m1_ratio)));
}
void Autonomous(void)
{
GetWatchdog().SetEnabled(false);
AverageWindowFilter<double, 20> fx, fy;
double ax, ay, lastAx = 0, lastAy = 0;
int state = 0;
while (IsAutonomous())
{
GetAcceleration(ax, ay);
fx.AddPoint( ax - avgX );
fy.AddPoint( ay - avgY );
ax = fx.GetAverage();
ay = fy.GetAverage();
switch (state)
{
case 0:
myRobot.Drive(1.0, 0.0);
if (fabs(ay - lastAy) > 1.0)
++state;
break;
case 1:
myRobot.Drive(-.5, 0.0);
Wait(3);
++state;
break;
case 2:
myRobot.Drive(0.0, 0.0);
break;
}
lastAx = ax;
lastAy = ay;
Wait(0.05);
}
}
void Humanoid::MoveInDirection(Vector *vMove)
{
SetMaxSpeed(vMove->Length());
if(*vMove!=Vector(0.0f, 0.0f, 0.0f))
{
if(vInternal.LengthSquared()<GetMaxSpeed()*GetMaxSpeed())
{
Vector vForce= *vMove/(GetAcceleration()*Timer::GetElapsedTime());
vInternal+=vForce;
}
}
if(!vMove->IsZeroVector())
FaceDirection(*vMove);
}
void CPointerAction::WriteProfileData(wxConfigBase* pConfObj)
{
pConfObj->SetPath (_T("pointerAction"));
pConfObj->Write(_T("xSpeed"), (long) GetXSpeed());
pConfObj->Write(_T("ySpeed"), (long) GetYSpeed());
pConfObj->Write(_T("acceleration"), (long) GetAcceleration());
pConfObj->Write(_T("smoothness"), (long) GetSmoothness());
pConfObj->Write(_T("easyStop"), (long) GetEasyStopValue());
pConfObj->Write(_T("enabledWorkspace"), (bool) GetRestrictedWorkingArea());
pConfObj->Write(_T("topWorkspace"), (long) GetTopWorkspace());
pConfObj->Write(_T("leftWorkspace"), (long) GetLeftWorkspace());
pConfObj->Write(_T("rightWorkspace"), (long) GetRightWorkspace());
pConfObj->Write(_T("bottomWorkspace"), (long) GetBottomWorkspace());
pConfObj->Write(_T("enabledWrapPointer"), (bool) GetWrapPointer());
pConfObj->Write(_T("clickMode"), (long) GetClickMode());
pConfObj->Write(_T("beepOnClick"), (bool) GetBeepOnClick());
m_pDwellClick->WriteProfileData(pConfObj);
m_pGestureClick->WriteProfileData(pConfObj);
pConfObj->SetPath (_T(".."));
}
// Handles slowing or speeding up the engines
void CHSSysEngines::ChangeSpeed()
{
HS_UINT32 uMax;
HS_UINT32 uAccel;
// Grab maximum velocity
uMax = GetMaxVelocity();
// If afterburning, max is increased by some ratio.
if (m_afterburning)
{
uMax *= m_iAfterburnRatio; //HSCONF.afterburn_ratio;
}
// Make sure the desired speed doesn't currently
// exceed the maximum speed limits.
if (m_desired_speed > uMax)
{
m_desired_speed = (float) uMax;
}
else if (m_desired_speed < (uMax * -.5))
{
m_desired_speed = uMax * (float) -.5;
}
// This has to come after the checking of uMax. Otherwise,
// the ship could be at top speed, power is taken away from
// engines, but the speed doesn't change because the desired
// and current speed are already equal.
if (m_desired_speed == m_current_speed)
{
return;
}
// Change the speed closer to the desired speed
if (m_current_speed > m_desired_speed)
{
// Grab deceleration rate, which is always the
// same as maximum acceleration. Deceleration is
// not affected by engine damage or power.
uAccel = GetAcceleration(false);
m_current_speed -= uAccel;
if (m_desired_speed < 0)
{
m_current_speed += m_desired_speed;
}
// Did we go too far?
if (m_current_speed < m_desired_speed)
{
m_current_speed = m_desired_speed;
}
}
else if (m_current_speed < m_desired_speed)
{
// Grab the acceleration rate, which is affected
// by power and damage.
uAccel = GetAcceleration(true);
m_current_speed += uAccel;
// Did we go too far?
if (m_current_speed > m_desired_speed)
{
m_current_speed = m_desired_speed;
}
}
}
HS_BOOL8 CHSSysEngines::GetAttributeValue(const HS_INT8 * pcAttrName,
CHSVariant & rvarValue, HS_BOOL8 bAdjusted, HS_BOOL8 bLocalOnly)
{
// Determine attribute, and return the value.
if (!_stricmp(pcAttrName, "EFFICIENCY"))
{
if (m_puiEfficiency)
{
rvarValue = *m_puiEfficiency;
}
else if (!bLocalOnly)
{
rvarValue = GetEfficiency();
}
else
{
return false;
}
return true;
}
else if (!_stricmp(pcAttrName, "MAX VELOCITY"))
{
if (m_puiMaxVelocity)
{
rvarValue = *m_puiMaxVelocity;
}
else if (!bLocalOnly)
{
rvarValue = GetMaxVelocity();
}
else
{
return false;
}
return true;
}
else if (!_stricmp(pcAttrName, "ACCELERATION"))
{
if (m_puiAcceleration)
{
rvarValue = *m_puiAcceleration;
}
else if (!bLocalOnly)
{
rvarValue = GetAcceleration();
}
else
{
return false;
}
return true;
}
else if (!_stricmp(pcAttrName, "DESIRED SPEED"))
{
rvarValue = m_desired_speed;
return true;
}
else if (!_stricmp(pcAttrName, "CURRENT SPEED"))
{
rvarValue = m_current_speed;
return true;
}
else if (!_stricmp(pcAttrName, "CAN AFTERBURN"))
{
if (m_pbCanAfterburn)
{
rvarValue = *m_pbCanAfterburn;
}
else if (!bLocalOnly)
{
rvarValue = CanBurn();
}
else
{
return false;
}
return true;
}
else if (!_stricmp(pcAttrName, "AFTERBURNING"))
{
rvarValue = m_afterburning;
return true;
}
else if (!_stricmp(pcAttrName, "AFTERBURN RATIO"))
{
rvarValue = m_iAfterburnRatio;
return true;
}
else
{
return CHSEngSystem::GetAttributeValue(pcAttrName, rvarValue,
bAdjusted, bLocalOnly);
}
}
/** {@inheritdoc} */
double ADXL345_I2C::GetZ() { return GetAcceleration(kAxis_Z); }
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void CBaseHelicopter::Flight( void )
{
if( GetFlags() & FL_ONGROUND )
{
//This would be really bad.
SetGroundEntity( NULL );
}
// Generic speed up
if (m_flGoalSpeed < GetMaxSpeed())
{
m_flGoalSpeed += GetAcceleration();
}
//NDebugOverlay::Line(GetAbsOrigin(), m_vecDesiredPosition, 0,0,255, true, 0.1);
// tilt model 5 degrees (why?! sjb)
QAngle vecAdj = QAngle( 5.0, 0, 0 );
// estimate where I'll be facing in one seconds
Vector forward, right, up;
AngleVectors( GetLocalAngles() + GetLocalAngularVelocity() * 2 + vecAdj, &forward, &right, &up );
// Vector vecEst1 = GetLocalOrigin() + GetAbsVelocity() + up * m_flForce - Vector( 0, 0, 384 );
// float flSide = DotProduct( m_vecDesiredPosition - vecEst1, right );
QAngle angVel = GetLocalAngularVelocity();
float flSide = DotProduct( m_vecDesiredFaceDir, right );
if (flSide < 0)
{
if (angVel.y < 60)
{
angVel.y += 8;
}
}
else
{
if (angVel.y > -60)
{
angVel.y -= 8;
}
}
angVel.y *= ( 0.98 ); // why?! (sjb)
// estimate where I'll be in two seconds
AngleVectors( GetLocalAngles() + angVel * 1 + vecAdj, NULL, NULL, &up );
Vector vecEst = GetAbsOrigin() + GetAbsVelocity() * 2.0 + up * m_flForce * 20 - Vector( 0, 0, 384 * 2 );
// add immediate force
AngleVectors( GetLocalAngles() + vecAdj, &forward, &right, &up );
Vector vecImpulse( 0, 0, 0 );
vecImpulse.x += up.x * m_flForce;
vecImpulse.y += up.y * m_flForce;
vecImpulse.z += up.z * m_flForce;
// add gravity
vecImpulse.z -= 38.4; // 32ft/sec
ApplyAbsVelocityImpulse( vecImpulse );
float flSpeed = GetAbsVelocity().Length();
float flDir = DotProduct( Vector( forward.x, forward.y, 0 ), Vector( GetAbsVelocity().x, GetAbsVelocity().y, 0 ) );
if (flDir < 0)
{
flSpeed = -flSpeed;
}
float flDist = DotProduct( GetDesiredPosition() - vecEst, forward );
// float flSlip = DotProduct( GetAbsVelocity(), right );
float flSlip = -DotProduct( GetDesiredPosition() - vecEst, right );
// fly sideways
if (flSlip > 0)
{
if (GetLocalAngles().z > -30 && angVel.z > -15)
angVel.z -= 4;
else
angVel.z += 2;
}
else
{
if (GetLocalAngles().z < 30 && angVel.z < 15)
angVel.z += 4;
else
angVel.z -= 2;
}
// These functions contain code Ken wrote that used to be right here as part of the flight model,
// but we want different helicopter vehicles to have different drag characteristics, so I made
// them virtual functions (sjb)
ApplySidewaysDrag( right );
ApplyGeneralDrag();
// apply power to stay correct height
// FIXME: these need to be per class variables
#define MAX_FORCE 80
#define FORCE_POSDELTA 12
#define FORCE_NEGDELTA 8
if (m_flForce < MAX_FORCE && vecEst.z < GetDesiredPosition().z)
{
m_flForce += FORCE_POSDELTA;
}
else if (m_flForce > 30)
{
if (vecEst.z > GetDesiredPosition().z)
m_flForce -= FORCE_NEGDELTA;
}
// pitch forward or back to get to target
//-----------------------------------------
// Pitch is reversed since Half-Life! (sjb)
//-----------------------------------------
if (flDist > 0 && flSpeed < m_flGoalSpeed /* && flSpeed < flDist */ && GetLocalAngles().x + angVel.x < 40)
{
// ALERT( at_console, "F " );
// lean forward
angVel.x += 12.0;
}
else if (flDist < 0 && flSpeed > -50 && GetLocalAngles().x + angVel.x > -20)
{
// ALERT( at_console, "B " );
// lean backward
angVel.x -= 12.0;
}
else if (GetLocalAngles().x + angVel.x < 0)
{
// ALERT( at_console, "f " );
angVel.x += 4.0;
}
else if (GetLocalAngles().x + angVel.x > 0)
{
// ALERT( at_console, "b " );
angVel.x -= 4.0;
}
SetLocalAngularVelocity( angVel );
// ALERT( at_console, "%.0f %.0f : %.0f %.0f : %.0f %.0f : %.0f\n", GetAbsOrigin().x, GetAbsVelocity().x, flDist, flSpeed, GetLocalAngles().x, m_vecAngVelocity.x, m_flForce );
// ALERT( at_console, "%.0f %.0f : %.0f %0.f : %.0f\n", GetAbsOrigin().z, GetAbsVelocity().z, vecEst.z, m_vecDesiredPosition.z, m_flForce );
}
void AnalogAccelerometer::UpdateTable() {
if (m_table != nullptr) {
m_table->PutNumber("Value", GetAcceleration());
}
}
bool operator!=(const Kinematic<T>& rhs)const
{
return GetVelocity() != rhs.GetVelocity() &&
GetAcceleration() != rhs.GetAcceleration();
}
void Accelerometer::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", GetAcceleration());
}
}
/** {@inheritdoc} */
double ADXL345_SPI::GetY()
{
return GetAcceleration(kAxis_Y);
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
double tm = GetTime();
//AccelerationReset();
AverageWindowFilter<double, 20> fx, fy;
GetWatchdog().SetEnabled(false);
double ax = 0, lastAx = 0;
double ay = 0, lastAy = 0;
for (int i = 0; i < 20; i++)
{
GetAcceleration(ax, ay);
fx.AddPoint(ax);
fy.AddPoint(ay);
Wait(0.05);
}
avgX = fx.GetAverage();
avgY = fy.GetAverage();
double minX = 0, maxX = 0;
double minY = 0, maxY = 0;
GetWatchdog().SetEnabled(true);
while (IsOperatorControl())
{
GetWatchdog().Feed();
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
if (GetTime() - tm > 0.05)
{
GetAcceleration(ax, ay);
fx.AddPoint( ax - avgX );
fy.AddPoint( ay - avgY );
ax = fx.GetAverage();
ay = fy.GetAverage();
minX = min(fabs(ax - lastAx), minX);
maxX = max(fabs(ax - lastAx), maxX);
minY = min(fabs(ay - lastAy), minY);
maxY = max(fabs(ay - lastAy), maxY);
lastAx = ax;
lastAy = ay;
//AccelerationUpdate( ax, ay, .1);
//get the filtered acceleration, velocity and position
//GetAcceleration( &ax, &ay);
// or
// ax = (((axH / 0.01) - .5) * 8.0) * 9.81;
// ay = (((ayH / 0.01) - .5) * 8.0) * 9.81;
DriverStationLCD * lcd = DriverStationLCD::GetInstance();
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "ax: %f m/s^2 ", ax);
lcd->Printf(DriverStationLCD::kUser_Line4, 1, "%.6f %.6f ", minX, maxX);
lcd->Printf(DriverStationLCD::kUser_Line5, 1, "ay: %f m/s^2 ", ay);
lcd->Printf(DriverStationLCD::kUser_Line6, 1, "%.6f %.6f ", minY, maxY);
// euler' integration method.. bad bad bad
//vx += ax / (0.01);
//vy += ay / (0.01);
//lcd->Printf(DriverStationLCD::kUser_Line4, 1, "vx: %.1f", vx);
//lcd->Printf(DriverStationLCD::kUser_Line6, 1, "vy: %.1f", vy);
lcd->UpdateLCD();
tm = GetTime();
}
}
}
/**
* Get the Acceleration for the PID Source parent.
*
* @return The current acceleration in Gs.
*/
double AnalogAccelerometer::PIDGet() { return GetAcceleration(); }
/**
* Get the Acceleration for the PID Source parent.
*
* @return The current acceleration in Gs.
*/
double FilteredAccelerometer::PIDGet()
{
return GetAcceleration();
}
