void
Spacetime::stepPhysics_analytic(matrix<double> u)
{
matrix<double> state_1 = getState();
matrix<double> C_1 = computeC_analytic();
matrix<double> G_1 = computeG_analytic();
matrix<double> MInv_1 = !computeM_analytic();
matrix<double> damping = G_1;//(MInv_1 * (u - C_1 - G_1));
damping(0,0) *= abs(state_1(2,0));
damping(1,0) *= abs(state_1(3,0));
matrix<double> thetaDotDot_1 = (MInv_1 * (u/*+damping*/ - C_1 - G_1));
matrix<double> stateDot_1(4,1);
stateDot_1(0,0) = state_1(2,0);
stateDot_1(1,0) = state_1(3,0);
stateDot_1(2,0) = thetaDotDot_1(0,0);
stateDot_1(3,0) = thetaDotDot_1(1,0);
setState(state_1 + stateDot_1*deltaT/2.0);
matrix<double> state_2 = getState();
matrix<double> C_2 = computeC_analytic();
matrix<double> G_2 = computeG_analytic();
matrix<double> MInv_2 = !computeM_analytic();
damping = G_2;
damping(0,0) *= abs(state_2(2,0));
damping(1,0) *= abs(state_2(3,0));
matrix<double> thetaDotDot_2 = (MInv_2 * (u/*+damping*/ - C_2 - G_2));
matrix<double> stateDot_2(4,1);
stateDot_2(0,0) = state_2(2,0);
stateDot_2(1,0) = state_2(3,0);
stateDot_2(2,0) = thetaDotDot_2(0,0);
stateDot_2(3,0) = thetaDotDot_2(1,0);
setState(state_1 + stateDot_2*deltaT);
}
SecondOrder::SecondOrder(int x,int y)
{
mdl=new RectangleView(x,y);
sNodeType="SecondOrder";
mosPath="Modelica.Blocks.Continuous.SecondOrder";
image_index = IM_SECONDORDER;
this->SetInputCount(1);
this->SetOutputCount(1);
this->SetInputsName(0, "u");
this->SetOutputsName(0, "y");
//参数初始化
para gain("k",1,"","二阶环节的增益"); //二阶环节的增益
para frequency("w",1,"","二阶环节的固有频率"); //二阶环节的固有频率
para damping("D",1,"","二阶环节的阻尼系数"); //二阶环节的阻尼系数
//加入参数列
para_List.push_back(gain);
para_List.push_back(frequency);
para_List.push_back(damping);
}
bool CPlayerStateSwim::OnPrePhysicsUpdate( CPlayer& player, const SActorFrameMovementParams& movement, float frameTime )
{
const CPlayerStateSwim_WaterTestProxy& waterProxy = player.m_playerStateSwim_WaterTestProxy;
CPlayerStateUtil::PhySetFly( player );
const SPlayerStats& stats = player.m_stats;
#ifdef STATE_DEBUG
const bool debug = (g_pGameCVars->cl_debugSwimming != 0);
#endif
const Vec3 entityPos = player.GetEntity()->GetWorldPos();
const Quat baseQuat = player.GetBaseQuat();
const Vec3 vRight(baseQuat.GetColumn0());
Vec3 velocity = player.GetActorPhysics().velocity;
// Underwater timer, sounds update and surface wave speed.
if (waterProxy.IsHeadUnderWater())
{
m_headUnderWaterTimer += frameTime;
if (m_headUnderWaterTimer <= -0.0f && !m_onSurface )
{
player.PlaySound(CPlayer::ESound_DiveIn, true, "speed", velocity.len());
m_headUnderWaterTimer = 0.0f;
}
player.PlaySound(CPlayer::ESound_Underwater, true);
}
else
{
m_headUnderWaterTimer -= frameTime;
if (m_headUnderWaterTimer >= 0.0f && (waterProxy.IsHeadComingOutOfWater() || m_onSurface))
{
player.PlaySound(CPlayer::ESound_DiveOut, true, "speed", velocity.len());
m_headUnderWaterTimer = 0.0f;
}
player.PlaySound(CPlayer::ESound_Underwater, false);
}
m_headUnderWaterTimer = clamp_tpl( m_headUnderWaterTimer, -0.2f, 0.2f );
// Apply water flow velocity to the player
Vec3 gravity;
pe_params_buoyancy buoyancy[s_maxWaterVolumesToConsider];
if (int count = gEnv->pPhysicalWorld->CheckAreas(entityPos, gravity, &buoyancy[0], s_maxWaterVolumesToConsider))
{
for(int i = 0; i < count && i < s_maxWaterVolumesToConsider; ++i)
{
// 0 is water
if( buoyancy[i].iMedium == 0 )
{
velocity += (buoyancy[i].waterFlow * frameTime);
}
}
}
// Calculate desired acceleration (user input)
Vec3 desiredWorldVelocity(ZERO);
Vec3 acceleration(ZERO);
{
Vec3 desiredLocalNormalizedVelocity(ZERO);
Vec3 desiredLocalVelocity(ZERO);
const Quat viewQuat = player.GetViewQuat();
const float backwardMultiplier = (float)__fsel(movement.desiredVelocity.y, 1.0f, g_pGameCVars->pl_swimBackSpeedMul);
desiredLocalNormalizedVelocity.x = movement.desiredVelocity.x * g_pGameCVars->pl_swimSideSpeedMul;
desiredLocalNormalizedVelocity.y = movement.desiredVelocity.y * backwardMultiplier;
float sprintMultiplier = 1.0f;
if ((player.IsSprinting()) && !player.IsCinematicFlagActive(SPlayerStats::eCinematicFlag_RestrictMovement))
{
sprintMultiplier = GetSwimParams().m_swimSprintSpeedMul;
// Higher speed multiplier when sprinting while looking up
const float upFraction = clamp_tpl(viewQuat.GetFwdZ(), 0.0f, 1.0f);
sprintMultiplier *= LERP(1.0f, GetSwimParams().m_swimUpSprintSpeedMul, upFraction);
}
const float baseSpeed = player.GetStanceMaxSpeed(STANCE_SWIM);
desiredLocalVelocity.x = desiredLocalNormalizedVelocity.x * sprintMultiplier * baseSpeed;
desiredLocalVelocity.y = desiredLocalNormalizedVelocity.y * sprintMultiplier * baseSpeed;
desiredLocalVelocity.z = desiredLocalNormalizedVelocity.z * g_pGameCVars->pl_swimVertSpeedMul * baseSpeed;
// The desired movement is applied in view-space, not in entity-space, since entity does not necessarily pitch while swimming.
desiredWorldVelocity += viewQuat.GetColumn0() * desiredLocalVelocity.x;
desiredWorldVelocity += viewQuat.GetColumn1() * desiredLocalVelocity.y;
// though, apply up/down in world space.
desiredWorldVelocity.z += desiredLocalVelocity.z;
#ifdef STATE_DEBUG
if (debug)
{
gEnv->pRenderer->DrawLabel(entityPos - vRight * 1.5f + Vec3(0,0,0.8f), 1.5f, "BaseSpeed %1.3f", baseSpeed);
gEnv->pRenderer->DrawLabel(entityPos - vRight * 1.5f + Vec3(0,0,1.0f), 1.5f, "SprintMul %1.2f", sprintMultiplier);
gEnv->pRenderer->DrawLabel(entityPos - vRight * 1.5f + Vec3(0,0,0.6f), 1.5f, "MoveN[%1.3f, %1.3f, %1.3f]", desiredLocalNormalizedVelocity.x, desiredLocalNormalizedVelocity.y, desiredLocalNormalizedVelocity.z);
gEnv->pRenderer->DrawLabel(entityPos - vRight * 1.5f + Vec3(0,0,0.5f), 1.5f, "VeloL[%1.3f, %1.3f, %1.3f]", desiredLocalVelocity.x, desiredLocalVelocity.y, desiredLocalVelocity.z);
gEnv->pRenderer->DrawLabel(entityPos - vRight * 1.5f + Vec3(0,0,0.4f), 1.5f, "VeloW[%1.3f, %1.3f, %1.3f]", desiredWorldVelocity.x, desiredWorldVelocity.y, desiredWorldVelocity.z);
}
#endif
//Remove a bit of control when entering the water
const float userControlFraction = (float)__fsel(0.3f - waterProxy.GetSwimmingTimer(), 0.2f, 1.0f);
acceleration += desiredWorldVelocity * userControlFraction;
}
// Apply acceleration (frame-rate independent)
const float accelerateDelayInv = 3.333f;
velocity += acceleration * (frameTime * accelerateDelayInv);
#ifdef STATE_DEBUG
if( debug )
{
gEnv->pRenderer->DrawLabel(entityPos - vRight * 1.5f - Vec3(0,0,0.2f), 1.5f, " Axx[%1.3f, %1.3f, %1.3f]", acceleration.x, acceleration.y, acceleration.z);
}
#endif
//--------------------
const float relativeWaterLevel = waterProxy.GetRelativeWaterLevel() + 0.1f;
const float surfaceDistanceFraction = clamp_tpl(fabsf(relativeWaterLevel), 0.0f, 1.0f);
float surfaceProximityInfluence = 1.0f - surfaceDistanceFraction;
const float verticalVelocityFraction = clamp_tpl((fabsf(desiredWorldVelocity.z) - 0.3f) * 2.5f, 0.0f, 1.0f);
surfaceProximityInfluence = surfaceProximityInfluence * (1.0f - verticalVelocityFraction);
// Apply velocity dampening (frame-rate independent)
Vec3 damping(ZERO);
const float zSpeedPreDamping = velocity.z;
{
damping.x = fabsf(velocity.x);
damping.y = fabsf(velocity.y);
// Vertical damping is special, to allow jumping out of water with higher speed,
// and also not sink too deep when falling down ito the water after jump or such.
float zDamp = 1.0f + (6.0f * clamp_tpl((-velocity.z - 1.0f) * 0.333f, 0.0f, 1.0f));
zDamp *= 1.0f - surfaceProximityInfluence;
damping.z = fabsf(velocity.z) * zDamp;
const float stopDelayInv = 3.333f;
damping *= (frameTime * stopDelayInv);
velocity.x = (float)__fsel((fabsf(velocity.x) - damping.x), (velocity.x - fsgnf(velocity.x) * damping.x), 0.0f);
velocity.y = (float)__fsel((fabsf(velocity.y) - damping.y), (velocity.y - fsgnf(velocity.y) * damping.y), 0.0f);
velocity.z = (float)__fsel((fabsf(velocity.z) - damping.z), (velocity.z - fsgnf(velocity.z) * damping.z), 0.0f);
//Make sure you can not swim above the surface
if ((relativeWaterLevel >= 0.0f) && (velocity.z > 0.0f))
{
velocity.z = 0.0f;
}
}
// Decide if we're on the surface and therefore need to be kept there..
if( relativeWaterLevel > -0.2f && relativeWaterLevel < 1.0f && fabs_tpl( zSpeedPreDamping ) < 0.5f )
{
if( !waterProxy.IsHeadUnderWater() )
{
m_onSurface = true;
}
}
else
{
// we only leave the surface if the player moves, otherwise we try and keep the
// player on the surface, even if they currently arent
m_onSurface = false;
}
// Calculate and apply surface movement to the player.
float speedDelta = 0.0f;
if( m_onSurface )
{
const float newWaterLevel = waterProxy.GetWaterLevel();
const float waterLevelDelta = clamp_tpl(newWaterLevel - m_lastWaterLevel, -1.0f, 1.0f );
const float newCheckedTime = waterProxy.GetWaterLevelTimeUpdated();
const float timeDelta = newCheckedTime - m_lastWaterLevelTime;
if( timeDelta > FLT_EPSILON )
{
speedDelta = waterLevelDelta/timeDelta;
velocity.z += speedDelta;
}
m_lastWaterLevel = newWaterLevel;
m_lastWaterLevelTime = newCheckedTime;
}
// Set request type and velocity
player.GetMoveRequest().type = eCMT_Fly;
player.GetMoveRequest().velocity = velocity;
#ifdef STATE_DEBUG
// DEBUG VELOCITY
if (debug)
{
gEnv->pRenderer->DrawLabel(entityPos - vRight * 1.5f - Vec3(0,0,0.0f), 1.5f, "Velo[%1.3f, %1.3f, %1.3f]", velocity.x, velocity.y, velocity.z);
gEnv->pRenderer->DrawLabel(entityPos - vRight * 1.5f - Vec3(0,0,0.4f), 1.5f, "Damp[%1.3f, %1.3f, %1.3f]", damping.x, damping.y, damping.z);
gEnv->pRenderer->DrawLabel(entityPos - vRight * 1.5f - Vec3(0,0,0.6f), 1.5f, "FrameTime %1.4f", frameTime);
gEnv->pRenderer->DrawLabel(entityPos - vRight * 1.5f + Vec3(0,0,0.3f), 1.5f, "DeltaSpeed[%1.3f]", speedDelta );
//if (bNewSwimJumping)
//gEnv->pRenderer->DrawLabel(entityPos - vRight * 0.15f + Vec3(0,0,0.6f), 2.0f, "JUMP");
}
#endif
return true;
}
