C++ Vec3::GetLength方法代码示例

void gkAuxRenderer::AuxRenderSkeleton( const Vec3& from, const Vec3& to, ColorF& color /*= ColorF(1.0,1.0,1.0,1.0)*/, float radius /*= 0.05f*/, bool ignoreZ /*= false */ )
{
	Vec3 dir = to - from;
	Vec3 dirInPlane = Vec3::CreateProjection(Vec3(0,0,1), dir.GetNormalized());
	if (dirInPlane.IsEquivalent(Vec3(0,0,0)))
	{
		dirInPlane = Vec3::CreateProjection(Vec3(1,0,0), dir.GetNormalized());
	}

	float len = dir.GetLength();
	dirInPlane.Normalize();
	dirInPlane *= radius * len;

	Vec3 dirInPlane1 = dirInPlane.GetRotated(dir.GetNormalized(), DEG2RAD(120.0f));
	Vec3 dirInPlane2 = dirInPlane.GetRotated(dir.GetNormalized(), DEG2RAD(-120.0f));

	Vec3 jointPt = from + dir.GetNormalized() * len * 0.8f;

	AuxRender3DLine(from, jointPt + dirInPlane, color, true);
	AuxRender3DLine(from, jointPt + dirInPlane1, color, true);
	AuxRender3DLine(from, jointPt + dirInPlane2, color, true);

	AuxRender3DLine(to, jointPt + dirInPlane, ColorF(1,0,0,1), true);
	AuxRender3DLine(to, jointPt + dirInPlane1, ColorF(1,0,0,1), true);
	AuxRender3DLine(to, jointPt + dirInPlane2, ColorF(1,0,0,1), true);

	AuxRender3DLine(jointPt + dirInPlane, jointPt + dirInPlane2, ColorF(1,0,0,1), true);
	AuxRender3DLine(jointPt + dirInPlane1, jointPt + dirInPlane2, ColorF(1,0,0,1), true);
	AuxRender3DLine(jointPt + dirInPlane, jointPt + dirInPlane1, ColorF(1,0,0,1), true);



}

void CIntersectionAssistanceUnit::DebugUpdate() const
{
    if(g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled)
        {
            IEntity* pEntity = gEnv->pEntitySystem->GetEntity(m_subjectEntityId);
            if(pEntity)
                {
                    IPhysicalEntity *pPhysical = pEntity->GetPhysics();
                    if(pPhysical)
                        {
                            const float fFontSize = 1.2f;
                            float drawColor[4] = {1.0f, 1.0f, 1.0f, 1.0f};

                            string sMsg(string().Format(" Entity ID: [%d]", m_subjectEntityId));
                            sMsg += string().Format("\n Entity Name: [%s]", pEntity->GetName());

                            sMsg += string().Format("\n EmbedTimer: [%.3f]", m_embedTimer);
                            sMsg += string().Format("\n EmbedState: [%s]",(m_embedState == eES_None) ? "NONE" : (m_embedState == eES_Evaluating) ? "EVALUATING" : (m_embedState == eES_ReEvaluating) ? "REEVALUATING" : (m_embedState == eES_NotEmbedded) ? "NOT EMBEDDED" : (m_embedState == eES_Embedded) ? "EMBEDDED" : "UNKNOWN");

                            Vec3 vCurrTrans = m_entityStartingWPos - pEntity->GetWorldPos();
                            sMsg += string().Format("\n Translation: < %.3f, %.3f, %.3f >", vCurrTrans.x, vCurrTrans.y, vCurrTrans.z );
                            sMsg += string().Format("\n Trans magnitude: < %.3f >", vCurrTrans.GetLength() );
                            sMsg += string().Format("\n Trans per sec: < %.3f >", vCurrTrans.GetLength() / g_pGameCVars->pl_pickAndThrow.intersectionAssistTimePeriod );

                            sMsg += string().Format("\n Collision count: %u", m_collisionCount );

                            // RENDER
                            Vec3 vDrawPos = pEntity->GetWorldPos() + Vec3(0.0f,0.0f,0.6f);
                            gEnv->pRenderer->DrawLabelEx(vDrawPos, fFontSize, drawColor, true, true, sMsg.c_str());

                            // Box
                            pe_params_bbox bbox;
                            if(pPhysical->GetParams(&bbox))
                                {
                                    ColorB colDefault = ColorB( 127,127,127 );
                                    ColorB embedded = ColorB(255, 0, 0);
                                    ColorB notEmbedded = ColorB(0, 255, 0);

                                    gEnv->pRenderer->GetIRenderAuxGeom()->DrawAABB( AABB(bbox.BBox[0],bbox.BBox[1]), Matrix34(IDENTITY), false, (m_embedState == eES_Embedded) ? embedded : (m_embedState == eES_NotEmbedded) ? notEmbedded : colDefault, eBBD_Faceted);
                                }
                        }
                }

        }
}

Quat QuatFromSourceToTarget( const Vec3& source, const Vec3& target )
{
    Vec3 s = source.GetNormalized();
    Vec3 t = target.GetNormalized();

    Vec3 c = s.GetCrossProduct( t );
    Real d = s.GetDotProduct( t );

    // check if vectors are the same
    if ( Equals( d, Real( 1 ), REAL_EPSILON * 3 ) )
    {
        // set to no rotation and return
        return Quat::ZERO;
    }

    // check for 180 degree rotation
    Real clen = c.GetLength();
    if ( clen <= REAL_EPSILON )
    {
        // pick an axis to rotate around
        Vec3 r = Vec3::UNIT_Y.GetCrossProduct( source );
        Real rlen = r.GetLength();

        if ( rlen <= REAL_EPSILON )
        {
            // bad luck, pick another axis
            r = Vec3::UNIT_X.GetCrossProduct( source );
            rlen = r.GetLength();
        }

        // normalize, set rotation and return
        r /= rlen;
        return QuatFromAxisAngle( r, Radian( REAL_PI ) );
    }

    // normalize c
    c /= clen;

    // get angle and set quaternion
    Real a = acos( d ) * Real( 0.5 );
    Real sa = sin( a );

    return Quat( cos( a ), c.X() * sa, c.Y() * sa, c.Z() * sa );
}

//====================================================================
// DebugDrawArrow
//====================================================================
void CAIDebugRenderer::DrawArrow(const Vec3& vPos, const Vec3& vLength, float fWidth, const ColorB& color)
{
	Vec3 points[7];
	Vec3 tris[5 * 3];

	float	len = vLength.GetLength();
	if (len < 0.0001f)
		return;

	float	headLen = fWidth * 2.0f;
	float	headWidth = fWidth * 2.0f;

	if (headLen > len * 0.8f)
		headLen = len * 0.8f;

	Vec3	vDir(vLength/len);
	Vec3	norm(vLength.y, -vLength.x, 0);
	norm.NormalizeSafe();

	Vec3	end(vPos + vLength);
	Vec3	start(vPos);

	unsigned int n = 0;
	points[n++] = end;
	points[n++] = end - vDir * headLen - norm * headWidth/2;
	PREFAST_SUPPRESS_WARNING(6386)
	points[n++] = end - vDir * headLen - norm * fWidth/2;
	points[n++] = end - vDir * headLen + norm * fWidth/2;
	points[n++] = end - vDir * headLen + norm * headWidth/2;
	points[n++] = start - norm * fWidth/2;
	points[n++] = start + norm * fWidth/2;

	n = 0;
	tris[n++] = points[0];
	tris[n++] = points[1];
	tris[n++] = points[2];

	tris[n++] = points[0];
	tris[n++] = points[2];
	tris[n++] = points[3];

	tris[n++] = points[0];
	tris[n++] = points[3];
	tris[n++] = points[4];

	tris[n++] = points[2];
	tris[n++] = points[5];
	tris[n++] = points[6];

	tris[n++] = points[2];
	tris[n++] = points[6];
	tris[n++] = points[3];

	DrawTriangles(tris, n, color);
}

Quat QuatFromExponentialMap( const Vec3& v )
{
    Radian angle( v.GetLength() );

    if ( angle > std::numeric_limits<Real>::epsilon() )
    {
        return QuatFromAxisAngle( v / angle, angle );
    }
    else
    {
        return Quat::ZERO;
    }
}

void CIntersectionAssistanceUnit::UpdateCheckingForObjectEmbed(const float dt)
{
    if(IsClientResponsibleForSubjectEntity())
        {
            if(m_embedTimer > 0.0f)
                {
                    m_embedTimer -= dt;
                    if(m_embedTimer < 0.0f)
                        {
                            IEntity* pEntity = gEnv->pEntitySystem->GetEntity(m_subjectEntityId);
                            if(pEntity)
                                {

                                    // Make our decision, is this object stuck?
                                    Vec3 translation = m_entityStartingWPos - pEntity->GetWorldPos();
                                    const float transPerSec = translation.GetLength() / g_pGameCVars->pl_pickAndThrow.intersectionAssistTimePeriod;
                                    const float countsPerSec  = m_collisionCount / g_pGameCVars->pl_pickAndThrow.intersectionAssistTimePeriod;

                                    if( countsPerSec >= g_pGameCVars->pl_pickAndThrow.intersectionAssistCollisionsPerSecond &&
                                            transPerSec <= g_pGameCVars->pl_pickAndThrow.intersectionAssistTranslationThreshold )
                                        {
                                            OnDetectObjectEmbedded();
                                        }
                                    else
                                        {
                                            // Are we still suspicious?
                                            if(countsPerSec >= g_pGameCVars->pl_pickAndThrow.intersectionAssistCollisionsPerSecond)
                                                {
                                                    // The object moved a fair distance, but its getting an awful lot of high penetration collision events - Test a while longer
                                                    BeginCheckingForObjectEmbed();
                                                    m_embedState = eES_ReEvaluating;
                                                }
                                            else
                                                {
                                                    m_embedState = eES_NotEmbedded;
                                                }
                                        }
                                }
                        }
                }
        }

#ifndef _RELEASE
    DebugUpdate();
#endif //#ifndef _RELEASE
}

void CBoidBug::UpdateBugsBehavior( float dt,SBoidContext &bc )
{
	if (cry_random(0, 9) == 0)
	{
		// Randomally modify heading vector.
		m_heading.x += Boid::Frand()*0.2f*bc.factorAlignment; // Used as random movement.
		m_heading.y += Boid::Frand()*0.2f*bc.factorAlignment;
		m_heading.z += Boid::Frand()*0.1f*bc.factorAlignment;
		m_heading = m_heading.GetNormalized();
		if (bc.behavior == EBUGS_DRAGONFLY)
			m_speed = bc.MinSpeed + (bc.MaxSpeed - bc.MinSpeed)*Boid::Frand();
	}

	// Avoid player.
	Vec3 fromPlayerVec = Vec3( m_pos.x-bc.playerPos.x, m_pos.y-bc.playerPos.y, 0 );
	if ((fromPlayerVec).GetLengthSquared() < BUGS_SCARE_DISTANCE*BUGS_SCARE_DISTANCE) // 2 meters.
	{
		float d = (BUGS_SCARE_DISTANCE - fromPlayerVec.GetLength());
		m_accel += 5.0f * fromPlayerVec * d;
	}

	// Maintain average speed.
	float targetSpeed = (bc.MaxSpeed + bc.MinSpeed)/2;
	m_accel -= m_heading*(m_speed-targetSpeed)*0.5f;

	//m_accel = (m_targetPos - m_pos)*bc.factorAttractToOrigin;

	if (m_pos.z < bc.terrainZ+bc.MinHeight)
	{
		m_accel.z = (bc.terrainZ+bc.MinHeight-m_pos.z)*bc.factorAttractToOrigin;
	}
	else if (m_pos.z > bc.terrainZ+bc.MaxHeight)
	{
		m_accel.z = -(m_pos.z-bc.terrainZ+bc.MinHeight)*bc.factorAttractToOrigin;
	}
	else
	{
		// Always try to accelerate in direction oposite to current in Z axis.
		m_accel.z += -m_heading.z * 0.2f;
	}
}

//------------------------------------------------------------------------
void CProjectile::SetVelocity(const Vec3 &pos, const Vec3 &dir, const Vec3 &velocity, float speedScale)
{
	if (!m_pPhysicalEntity)
		return;

	Vec3 totalVelocity = (dir * m_pAmmoParams->speed * speedScale) + velocity;

	if (m_pPhysicalEntity->GetType()==PE_PARTICLE)
	{
		pe_params_particle particle;
		particle.heading = totalVelocity.GetNormalized();
		particle.velocity = totalVelocity.GetLength();

		m_pPhysicalEntity->SetParams(&particle);
	}
	else if (m_pPhysicalEntity->GetType()==PE_RIGID)
	{
		pe_action_set_velocity vel;
		vel.v = totalVelocity;

		m_pPhysicalEntity->Action(&vel);
	}
}

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

				if (m_stopping)
				{
					m_stopping = false;
					pActInfo->pGraph->SetRegularlyUpdated( pActInfo->myID, false );

					if( pPhysEnt == NULL )
					{
						SetPos(pActInfo, m_destination);
					}

					m_bActive = false;
					break;
				}
				
				if (!m_bActive) break;
				float time = gEnv->pTimer->GetFrameStartTime().GetSeconds();
				float timeDifference = time - m_lastFrameTime; 
				m_lastFrameTime = time;

					// note - if there's no physics then this will be the same, but if the platform is moved through physics, then
					//        we have to get back the actual movement - this maybe framerate dependent.
				m_position = pActInfo->pEntity->GetPos();

				// let's compute the movement vector now
				Vec3 oldPosition = m_position;

				if(m_bForceFinishAsTooNear || m_position.IsEquivalent(m_destination, 0.01f))
				{
					m_position = m_destination;
					oldPosition = m_destination;
					ActivateOutput(pActInfo, OUT_DONE, true);
					ActivateOutput(pActInfo, OUT_FINISH, true);
					SetPos(pActInfo, m_position); // for finishing we have to make a manual setpos even if there is physics involved, to make sure the entity will be where it should.
					if (pPhysEnt)
					{
						pe_action_set_velocity setVel;
						setVel.v = ZERO;
						pPhysEnt->Action( &setVel );

						m_stopping = true;
					}
					else
					{
						pActInfo->pGraph->SetRegularlyUpdated( pActInfo->myID, false );
						m_bActive = false;
					}
				}
				else
				{
					Vec3 direction = m_destination - m_position;
					float distance = direction.GetLength();
					Vec3 directionAndSpeed = direction.normalized();

					// ease-area calcs
					float distanceForEaseOutCalc = distance + m_easeOutDistance * EASE_MARGIN_FACTOR;
					if (distanceForEaseOutCalc < m_easeOutDistance)	// takes care of m_easeOutDistance being 0
					{
						directionAndSpeed *= distanceForEaseOutCalc / m_easeOutDistance;
					}
					else  // init code makes sure both eases dont overlap, when the movement is time defined. when it is speed defined, only ease out is applied if they overlap. 
					{
						if (m_easeInDistance>0.f)
						{
							Vec3 vectorFromStart = m_position - m_startPos;
							float distanceFromStart = vectorFromStart.GetLength();
							float distanceForEaseInCalc = distanceFromStart + m_easeInDistance * EASE_MARGIN_FACTOR;
							if (distanceForEaseInCalc < m_easeInDistance)
							{
								directionAndSpeed *= distanceForEaseInCalc / m_easeInDistance;
							}
						}
					}

					directionAndSpeed *= (m_topSpeed * timeDifference);

					if(direction.GetLength() < directionAndSpeed.GetLength())
					{
						m_position = m_destination;
						m_bForceFinishAsTooNear = true;
					}
					else
						m_position += directionAndSpeed;
				}
				ActivateOutput(pActInfo, OUT_CURRENT, m_position);
				if (pPhysEnt == NULL)
				{
					SetPos(pActInfo, m_position);
				}
				else
				{
					pe_action_set_velocity setVel;
					float rTimeStep = timeDifference>0.000001f ? 1.f / timeDifference : 0.0f;
					setVel.v = (m_position - oldPosition) * rTimeStep;  
					pPhysEnt->Action( &setVel );
				}
				break;
			}
		};
	};

void CPlayerStateUtil::CalculateGroundOrJumpMovement( const CPlayer& player, const SActorFrameMovementParams &movement, const bool bigWeaponRestrict, Vec3 &move )
{
	const bool isPlayer = player.IsPlayer();
	const float totalMovement = fabsf(movement.desiredVelocity.x) + fabsf(movement.desiredVelocity.y);
	const bool moving = (totalMovement > FLT_EPSILON);

	if (moving)
	{	
		Vec3 desiredVelocityClamped = movement.desiredVelocity;
		const float desiredVelocityMag = desiredVelocityClamped.GetLength();

		const float invDesiredVelocityMag = 1.0f / desiredVelocityMag;

		float strafeMul;

		if (bigWeaponRestrict)
		{
			strafeMul = g_pGameCVars->pl_movement.nonCombat_heavy_weapon_strafe_speed_scale;
		}
		else
		{
			strafeMul = g_pGameCVars->pl_movement.strafe_SpeedScale;
		}

		float backwardMul = 1.0f;

		desiredVelocityClamped = desiredVelocityClamped * (float)__fsel(-(desiredVelocityMag - 1.0f), 1.0f, invDesiredVelocityMag);

		//going back?
		if (isPlayer)	//[Mikko] Do not limit backwards movement when controlling AI.
		{
			backwardMul = (float)__fsel(desiredVelocityClamped.y, 1.0f, LERP(backwardMul, player.m_params.backwardMultiplier, -desiredVelocityClamped.y));
		}

		NETINPUT_TRACE(player.GetEntityId(), backwardMul);
		NETINPUT_TRACE(player.GetEntityId(), strafeMul);

		const Quat oldBaseQuat = player.GetEntity()->GetWorldRotation(); // we cannot use m_baseQuat: that one is already updated to a new orientation while desiredVelocity was relative to the old entity frame
		move += oldBaseQuat.GetColumn0() * desiredVelocityClamped.x * strafeMul * backwardMul;
		move += oldBaseQuat.GetColumn1() * desiredVelocityClamped.y * backwardMul;
	}

	//ai can set a custom sprint value, so dont cap the movement vector
	if (movement.sprint<=0.0f)
	{
		//cap the movement vector to max 1
		float moveModule(move.len());

		//[Mikko] Do not limit backwards movement when controlling AI, otherwise it will disable sprinting.
		if (isPlayer)
		{
			move /= (float)__fsel(-(moveModule - 1.0f), 1.0f, moveModule);
		}

		NETINPUT_TRACE(player.GetEntityId(), moveModule);
	}

	//player movement don't need the m_frameTime, its handled already in the physics
	float scale = player.GetStanceMaxSpeed(player.GetStance());
	move *= scale;
	NETINPUT_TRACE(player.GetEntityId(), scale);

	if (isPlayer)
	{
		const bool  isCrouching = ((player.m_actions & ACTION_CROUCH) != 0);
		AdjustMovementForEnvironment( player, move, bigWeaponRestrict, isCrouching );
	}
}

//////////////////////////////////////////////////////////////////////////
// NOTE: This function must be thread-safe. Before adding stuff contact MarcoC.
void CVehicleMovementTank::ProcessAI(const float deltaTime)
{
	FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );

	float dt = max( deltaTime, 0.005f);

	m_movementAction.brake = false;
	m_movementAction.rotateYaw = 0.0f;
	m_movementAction.power = 0.0f;

	float inputSpeed = 0.0f;
	{
		if (m_aiRequest.HasDesiredSpeed())
			inputSpeed = m_aiRequest.GetDesiredSpeed();
		Limit(inputSpeed, -m_maxSpeed, m_maxSpeed);
	}

	Vec3 vMove(ZERO);
	{
		if (m_aiRequest.HasMoveTarget())
			vMove = ( m_aiRequest.GetMoveTarget() - m_PhysPos.pos ).GetNormalizedSafe();
	}

	//start calculation
	if ( fabsf( inputSpeed ) < 0.0001f || m_tireBlownTimer > 1.5f )
	{
		// only the case to use a hand break
		m_movementAction.brake = true;
	}
	else
	{

		Matrix33 entRotMatInvert( m_PhysPos.q );
		entRotMatInvert.Invert();
		float currentAngleSpeed = RAD2DEG(-m_PhysDyn.w.z);

		const static float maxSteer = RAD2DEG(gf_PI/4.f); // fix maxsteer, shouldn't change  
		Vec3 vVel = m_PhysDyn.v;
		Vec3 vVelR = entRotMatInvert * vVel;
		float currentSpeed =vVel.GetLength();
		vVelR.NormalizeSafe();
		if ( vVelR.Dot( FORWARD_DIRECTION ) < 0 )
			currentSpeed *= -1.0f;

		// calculate pedal
		static float accScale = 0.5f;
		m_movementAction.power = (inputSpeed - currentSpeed) * accScale;
		Limit( m_movementAction.power, -1.0f, 1.0f);

		// calculate angles
		Vec3 vMoveR = entRotMatInvert * vMove;
		Vec3 vFwd	= FORWARD_DIRECTION;

		vMoveR.z =0.0f;
		vMoveR.NormalizeSafe();

		if ( inputSpeed < 0.0 )	// when going back
		{
			vFwd *= -1.0f;
			vMoveR *= -1.0f;
			currentAngleSpeed *=-1.0f;
		}

		float cosAngle = vFwd.Dot(vMoveR);
		float angle = RAD2DEG( acos_tpl(cosAngle));
		if ( vMoveR.Dot( Vec3( 1.0f,0.0f,0.0f ) )< 0 )
			angle = -angle;

		//		int step =0;
		m_movementAction.rotateYaw = angle * 1.75f/ maxSteer; 

		// implementation 1. if there is enough angle speed, we don't need to steer more
		if ( fabsf(currentAngleSpeed) > fabsf(angle) && angle*currentAngleSpeed > 0.0f )
		{
			m_movementAction.rotateYaw = m_currSteer*0.995f; 
			//			step =1;
		}

		// implementation 2. if we can guess we reach the distination angle soon, start counter steer.
		float predictDelta = inputSpeed < 0.0f ? 0.1f : 0.07f;
		float dict = angle + predictDelta * ( angle - m_prevAngle) / dt ;
		if ( dict*currentAngleSpeed<0.0f )
		{
			if ( fabsf( angle ) < 2.0f )
			{
				m_movementAction.rotateYaw = angle* 1.75f/ maxSteer; 
				//				step =3;
			}
			else
			{
				m_movementAction.rotateYaw = currentAngleSpeed < 0.0f ? 1.0f : -1.0f; 
				//				step =2;
			}
		}

		// implementation 3. tank can rotate at a point
		if ( fabs( angle )> 20.0f ) 
		{
//.........这里部分代码省略.........

//////////////////////////////////////////////////////////////////////////
// NOTE: This function must be thread-safe. Before adding stuff contact MarcoC.
void CVehicleMovementVTOL::ProcessAI(const float deltaTime)
{
	FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );
	
	if (!m_isVTOLMovement)
	{
		CVehicleMovementHelicopter::ProcessAI(deltaTime);
		return;
	}

	m_velDamp = 0.15f;

	const float maxDirChange = 15.0f;

	// it's useless to progress further if the engine has yet to be turned on
	if (!m_isEnginePowered)
		return;

	m_movementAction.Clear();
	m_movementAction.isAI = true;
	ResetActions();

	// Our current state
	const Vec3 worldPos =  m_PhysPos.pos;
	const Matrix33 worldMat( m_PhysPos.q);
	Ang3 worldAngles = Ang3::GetAnglesXYZ(worldMat);

	const Vec3 currentVel = m_PhysDyn.v;
	const Vec3 currentVel2D(currentVel.x, currentVel.y, 0.0f);
	// +ve direction mean rotation anti-clocwise about the z axis - 0 means along y
	float currentDir = worldAngles.z;

	// to avoid singularity
	const Vec3 vWorldDir = worldMat * FORWARD_DIRECTION;
	const Vec3 vWorldDir2D =  Vec3( vWorldDir.x,  vWorldDir.y, 0.0f ).GetNormalizedSafe();

	// Our inputs
	const float desiredSpeed = m_aiRequest.HasDesiredSpeed() ? m_aiRequest.GetDesiredSpeed() : 0.0f;
	const Vec3 desiredMoveDir = m_aiRequest.HasMoveTarget() ? (m_aiRequest.GetMoveTarget() - worldPos).GetNormalizedSafe() : vWorldDir;
	const Vec3 desiredMoveDir2D = Vec3(desiredMoveDir.x, desiredMoveDir.y, 0.0f).GetNormalizedSafe(vWorldDir2D);
	const Vec3 desiredVel = desiredMoveDir * desiredSpeed; 
	const Vec3 desiredVel2D(desiredVel.x, desiredVel.y, 0.0f);
	const Vec3 desiredLookDir = m_aiRequest.HasLookTarget() ? (m_aiRequest.GetLookTarget() - worldPos).GetNormalizedSafe() : desiredMoveDir;
	const Vec3 desiredLookDir2D = Vec3(desiredLookDir.x, desiredLookDir.y, 0.0f).GetNormalizedSafe(vWorldDir2D);

	// Calculate the desired 2D velocity change
	Vec3 desiredVelChange2D = desiredVel2D - currentVel2D;
	float velChangeLength = desiredVelChange2D.GetLength2D();

	bool isLandingMode = false;
	if (m_pLandingGears && m_pLandingGears->AreLandingGearsOpen())
		isLandingMode = true;

	bool isHorizontal = (desiredSpeed >= 5.0f) && (desiredMoveDir.GetLength2D() > desiredMoveDir.z);
	float desiredPitch = 0.0f;
	float desiredRoll = 0.0f;

	float desiredDir = atan2f(-desiredLookDir2D.x, desiredLookDir2D.y);

	while (currentDir < desiredDir - gf_PI)
		currentDir += 2.0f * gf_PI;
	while (currentDir > desiredDir + gf_PI)
		currentDir -= 2.0f * gf_PI;

	float diffDir = (desiredDir - currentDir);
	m_actionYaw = diffDir * m_yawInputConst;
	m_actionYaw += m_yawInputDamping * (currentDir - m_lastDir) / deltaTime;
	m_lastDir = currentDir;

	if (isHorizontal && !isLandingMode)
	{
		float desiredFwdSpeed = desiredVelChange2D.GetLength();

		desiredFwdSpeed *= min(1.0f, diffDir / DEG2RAD(maxDirChange));

		if (!iszero(desiredFwdSpeed))
		{
			const Vec3 desiredWorldTiltAxis = Vec3(-desiredVelChange2D.y, desiredVelChange2D.x, 0.0f);
			const Vec3 desiredLocalTiltAxis = worldMat.GetTransposed() * desiredWorldTiltAxis;

			m_forwardAction = m_fwdPID.Update(currentVel.y, desiredLocalTiltAxis.GetLength(), -1.0f, 1.0f);

		float desiredTiltAngle = m_tiltPerVelDifference * desiredVelChange2D.GetLength();
		Limit(desiredTiltAngle, -m_maxTiltAngle, m_maxTiltAngle);

		if (desiredTiltAngle > 0.0001f)
		{
			const Vec3 desiredWorldTiltAxis2 = Vec3(-desiredVelChange2D.y, desiredVelChange2D.x, 0.0f).GetNormalizedSafe();
			const Vec3 desiredLocalTiltAxis2 = worldMat.GetTransposed() * desiredWorldTiltAxis2;

			Vec3 vVelLocal = worldMat.GetTransposed() * desiredVel;
			vVelLocal.NormalizeSafe();

			float dotup = vVelLocal.Dot(Vec3( 0.0f,0.0f,1.0f ) );
			float currentSpeed = currentVel.GetLength();

			desiredPitch = dotup *currentSpeed / 100.0f;
			desiredRoll = desiredTiltAngle * desiredLocalTiltAxis2.y *currentSpeed/30.0f;
//.........这里部分代码省略.........

void CNetLerper::Update(float dt, const Vec3& entityPos, SPrediction& predictionOut, const Vec3& velGround, bool bInAirOrJumping)
{
	if(!m_enabled)
	{
		predictionOut.predictedPos = entityPos;
		predictionOut.lerpVel.zero();
		predictionOut.shouldSnap = false;
		return;
	}

	CRY_ASSERT(m_settings);
	dt = max(dt, 0.001f);
	
	IEntity* pGroundEntity = gEnv->pEntitySystem->GetEntity(m_standingOn);
	m_desired.worldPos = m_desired.pos;
	if (pGroundEntity)
	{
		if (IPhysicalEntity* pGroundPhys = pGroundEntity->GetPhysics())
		{
			pe_status_pos psp;
			pGroundPhys->GetStatus(&psp);
			m_desired.worldPos = psp.q * m_desired.pos + psp.pos;
		}
	}

	// Prediction is done a "long" time ahead
	const float predictTime = min(m_clock + m_settings->lookAhead, m_settings->maxLookAhead);
	const Vec3 predictedPos = m_desired.worldPos + (m_desired.vel * predictTime);
	const Vec3 predOffset = predictedPos - entityPos;
	const float predDist = predOffset.GetLength();

	// Errors:
	m_lerpedError = entityPos - predictedPos;
	// Error between desired pos (nb: not predicted pos)
	const Vec3 errorToDesiredPos = entityPos - m_desired.worldPos;
	const int snapError = GetLerpError(errorToDesiredPos, m_lerpedError);

	m_clock += dt;

	const float lerpDist = predDist + (dt*velGround).GetLength();

	if (lerpDist<m_settings->minLerpDistance && m_desired.vel.GetLengthSquared() < sqr(m_settings->minLerpSpeed) && !bInAirOrJumping)  // Stop lerping
	{
		// This block should be entered as few times as possible while on a moving platform.
		predictionOut.predictedPos = predictedPos;
		predictionOut.lerpVel.zero();
		predictionOut.shouldSnap = false;
		m_lerpedPos = m_desired.worldPos;
		m_lerpedError.zero();

		if (m_snapType== eSnap_None)
		{
			predictionOut.shouldSnap = true;
			m_snapType = eSnap_Minor;
			LogSnapError();
		}
	}
	else if (snapError & k_desiredError)  // k_lerpError is ignored because it works improperly during collisions with living entities
	{
		predictionOut.predictedPos = m_desired.worldPos;
		predictionOut.lerpVel.zero();
		predictionOut.shouldSnap = true;
		m_lerpedPos = m_desired.worldPos;
		m_lerpedError.zero();
		if(errorToDesiredPos.GetLengthSquared() > sqr(m_settings->snapDistMarkedMajor))
		{
			m_snapType = eSnap_Major;
		}
		else
		{
			m_snapType = eSnap_Normal;
		}
		LogSnapError();
	}
	else 
	{
		// Calculate simple lerp velocity
		Vec3 lerpVel = predOffset * (float)__fres(m_settings->lookAhead);
			
		// Clamp it
		const float maxPredictionDistance = m_settings->maxInterSpeed * m_settings->lookAhead;
		if (predDist > maxPredictionDistance)
			lerpVel *= maxPredictionDistance * (float)__fres(predDist);

		// Output
		predictionOut.predictedPos = predictedPos;
		predictionOut.lerpVel = lerpVel;
		predictionOut.shouldSnap = false;
		m_snapType = eSnap_None;
	}

	// Keep this in local space
	m_lerpedPos += dt * (predictionOut.lerpVel + velGround);
	m_lerpedPos += (m_desired.worldPos - m_lerpedPos) * 0.05f;	// Keep on top of any drift
}

void CFireModePlugin_AutoAim::AdjustFiringDirection( const Vec3& attackerPos, Vec3& firingDirToAdjust, const bool bCurrentlyZoomed, const EntityId ownerId ) const
{
	CPlayer* pAttackingPlayer = static_cast<CPlayer*>(g_pGame->GetIGameFramework()->GetIActorSystem()->GetActor(ownerId));
	if (pAttackingPlayer && pAttackingPlayer->IsPlayer())
	{
		const ConeParams& aimConeSettings = GetAimConeSettings(bCurrentlyZoomed);

		// Don't do any projectile adjusting if the player already has a target for themselves, and is on target (e.g. manually scoping to get a headshot)	
		if( m_targetSelectionParams.m_bDisableAutoAimIfPlayerAlreadyHasTarget && pAttackingPlayer->GetCurrentTargetEntityId() ||
			!aimConeSettings.m_enabled)
		{

#if ALLOW_PROJECTILEHELPER_DEBUGGING
			m_lastShotAutoAimedStatus.append("FALSE - [Reason]: Player already on target");
#endif //#if #endif //#if ALLOW_PROJECTILEHELPER_DEBUGGING

			return;
		}

		float incomingDirLength = firingDirToAdjust.GetLength();
		CRY_ASSERT(incomingDirLength>0.f);
		Vec3 firingDirToAdjustNorm(firingDirToAdjust*__fres(incomingDirLength));

	#if ALLOW_PROJECTILEHELPER_DEBUGGING
		// DEBUG RENDER
		if (g_pGameCVars->pl_debug_projectileAimHelper)
		{
			// Draw Target acquisition cone
			float length = aimConeSettings.m_maxDistance;
			float radius = length * tan(aimConeSettings.m_outerConeRads * 0.5f);

			SAuxGeomRenderFlags originalFlags = gEnv->pRenderer->GetIRenderAuxGeom()->GetRenderFlags();
			SAuxGeomRenderFlags newFlags = originalFlags;
			newFlags.SetCullMode(e_CullModeNone);
			newFlags.SetFillMode(e_FillModeWireframe);
			newFlags.SetAlphaBlendMode(e_AlphaBlended);

			gEnv->pRenderer->GetIRenderAuxGeom()->SetRenderFlags(newFlags);
			gEnv->pRenderer->GetIRenderAuxGeom()->DrawCone(attackerPos + (firingDirToAdjustNorm*aimConeSettings.m_maxDistance),-firingDirToAdjustNorm, radius , length , ColorB(132,190,255,120), true );

			// Draw projectile adjust cone
			radius = length * tan(aimConeSettings.m_innerConeRads * 0.5f);
			gEnv->pRenderer->GetIRenderAuxGeom()->DrawCone(attackerPos + (firingDirToAdjustNorm*aimConeSettings.m_maxDistance),-firingDirToAdjustNorm, radius , length ,ColorB(0,0,127,120), true );
		
			// Restore render flags
			gEnv->pRenderer->GetIRenderAuxGeom()->SetRenderFlags(originalFlags);
		}
	#endif //#if ALLOW_PROJECTILEHELPER_DEBUGGING

		IEntity* pTargetPlayer = CalculateBestProjectileAutoAimTarget(attackerPos, firingDirToAdjustNorm, bCurrentlyZoomed, ownerId);
		if(pTargetPlayer)
		{
			const SAutoaimTarget* pAutoAimInfo = g_pGame->GetAutoAimManager().GetTargetInfo(pTargetPlayer->GetId());
			if(pAutoAimInfo)
			{
				Vec3 desiredFiringDir = ( pAutoAimInfo->primaryAimPosition - attackerPos ).GetNormalized();

				// Make sure final firing dir still constrained to valid cone
				float vecDot = firingDirToAdjustNorm.Dot(desiredFiringDir);
				float maxConeAngle = cos(0.5f * aimConeSettings.m_innerConeRads);
				if(vecDot >= maxConeAngle)
				{
					// within cone
					firingDirToAdjustNorm = desiredFiringDir;

#if ALLOW_PROJECTILEHELPER_DEBUGGING
					m_lastShotAutoAimedStatus.append("TRUE + desired dir fully WITHIN allowed adjust cone");
#endif //#if ALLOW_PROJECTILEHELPER_DEBUGGING

				}
				else
				{
					// constrain (generally working with small angles, nLerp should be fine + cheap)
					const float invConeDot = 1.0f - maxConeAngle;
					const float invVecDot  = 1.0f - vecDot;
					float zeroToOne  = invConeDot / invVecDot;
					Vec3 finalVec	  = (zeroToOne * desiredFiringDir) + ((1.0f - zeroToOne) * firingDirToAdjustNorm);
					finalVec.Normalize();
					firingDirToAdjustNorm = finalVec;

#if ALLOW_PROJECTILEHELPER_DEBUGGING
					m_lastShotAutoAimedStatus.Format("TRUE + desired dir CONSTRAINED to allowed cone [desired]: %.3f deg [constrained To]: %.3f deg", RAD2DEG(acos(vecDot)), 0.5f * RAD2DEG(aimConeSettings.m_innerConeRads));
#endif //#if ALLOW_PROJECTILEHELPER_DEBUGGING
				}
				
			}
		}
#if ALLOW_PROJECTILEHELPER_DEBUGGING
		else
		{
			m_lastShotAutoAimedStatus.Format("FALSE - CalculateBestProjectileAutoAimTarget NULL [Last reason]: %s", m_lastTargetRejectionReason.c_str());
		}

		// Draw adjusted vec
		if (g_pGameCVars->pl_debug_projectileAimHelper)
		{
			float length = aimConeSettings.m_maxDistance;
			gEnv->pRenderer->GetIRenderAuxGeom()->DrawLine(attackerPos,ColorB(255,0,255,0),attackerPos + firingDirToAdjustNorm * length,ColorB(255,0,255,0));
		}
#endif //#if ALLOW_PROJECTILEHELPER_DEBUGGING
//.........这里部分代码省略.........

int CFlowConvoyNode::OnPhysicsPostStep(const EventPhys * pEvent)
{
	FUNCTION_PROFILER(GetISystem(), PROFILE_GAME);

  if(m_bFirstUpdate)
    return 0; //after QuickLoad OnPhysicsPostStep called before ProcessEvent

	const EventPhysPostStep *pPhysPostStepEvent = (const EventPhysPostStep *)pEvent;

	IPhysicalEntity *pEventPhysEnt = pPhysPostStepEvent->pEntity;

	Vec3 posBack, posFront, posCenter;

	for (size_t i = 0; i < m_coaches.size(); ++i)
	{
		IPhysicalEntity *pPhysEnt = m_coaches[i].m_pEntity->GetPhysics();

		if (pEventPhysEnt == pPhysEnt)
		{
			if (m_ShiftTime > 0.0f)
			{
				m_speed = m_speed * (1.0f - breakValue * pPhysPostStepEvent->dt / m_MaxShiftTime);
				m_ShiftTime -= pPhysPostStepEvent->dt;
			}
			else
			{
				m_speed = m_speed + min(1.0f, (pPhysPostStepEvent->dt / 5.0f)) * (m_desiredSpeed - m_speed);
			}
		

			float speed = (m_splitCoachIndex > 0 && (int)i >= m_splitCoachIndex) ? m_splitSpeed : m_speed;
			float distance = (m_coaches[i].m_distanceOnPath += pPhysPostStepEvent->dt * speed);
			
			if(m_splitCoachIndex>0)
			{//train splitted
				if(i==m_splitCoachIndex-1) // update m_distanceOnPath for serialization
					m_distanceOnPath=distance-m_coaches[i].m_coachOffset;
				else if(i==m_coaches.size()-1) // update m_splitDistanceOnPath for serialization
					m_splitDistanceOnPath=distance-m_coaches[i].m_coachOffset;
			}
			else
			{//train in one piece
				if(i==m_coaches.size()-1)// update m_distanceOnPath for serialization
					m_distanceOnPath=distance-m_coaches[i].m_coachOffset;
			}

			posBack  = m_path.GetPointAlongPath(distance - m_coaches[i].m_wheelDistance, m_coaches[i].m_backWheelIterator[0]);
			posFront = m_path.GetPointAlongPath(distance + m_coaches[i].m_wheelDistance, m_coaches[i].m_frontWheelIterator[0]);
			posCenter = (posBack+posFront)*0.5f;


			Vec3 posDiff = posFront - posBack;
			float magnitude = posDiff.GetLength();

			if (magnitude > FLT_EPSILON)
			{
				posDiff *= 1.0f / magnitude;

				pe_params_pos ppos;
				ppos.pos = posCenter;
				ppos.q = Quat::CreateRotationVDir(posDiff);
				if (m_bXAxisFwd)
					ppos.q *= Quat::CreateRotationZ(gf_PI*-0.5f);
				pPhysEnt->SetParams(&ppos, 0 /* bThreadSafe=0 */); // as we are calling from the physics thread

				Vec3 futurePositionBack, futurePositionFront;
				futurePositionBack  = m_path.GetPointAlongPath(distance + PATH_DERIVATION_TIME * speed - m_coaches[i].m_wheelDistance, m_coaches[i].m_backWheelIterator[1]);
				futurePositionFront = m_path.GetPointAlongPath(distance + PATH_DERIVATION_TIME * speed + m_coaches[i].m_wheelDistance, m_coaches[i].m_frontWheelIterator[1]);

				Vec3 futurePosDiff = futurePositionFront - futurePositionBack;
				magnitude = futurePosDiff.GetLength();

				if (magnitude > FLT_EPSILON)
				{
					futurePosDiff *= 1.0f / magnitude;

					pe_action_set_velocity setVel;
					setVel.v = ((futurePositionBack+ futurePositionFront)*0.5f - posCenter) * (1.0f/PATH_DERIVATION_TIME);

					//Vec3 dir=(posFront-posBack).GetNormalized();
					//Vec3 future_dir=(futurePositionFront-futurePositionBack).GetNormalized();
					Vec3 w=posDiff.Cross(futurePosDiff);
					float angle=cry_asinf(w.GetLength());
					setVel.w=(angle/PATH_DERIVATION_TIME)*w.GetNormalized();
					pPhysEnt->Action(&setVel, 0 /* bThreadSafe=0 */); // as we are calling from the physics thread
					break;
				}
			}
		}
	}

	// Done processing once we reach end of path
	if (m_atEndOfPath)
		m_processNode = false;

	return 0;
}