private void UpdateMovement(GameThing thing, float secs)
{
Movement movement = thing.Movement;
// Update target position if homing on a moving object.
if (Homing)
{
targetPosition = targetObject.WorldCollisionCenter;
GameActor.AddMissileLine(thing, targetObject);
}
Vector3 collisionCenter = Vector3.Transform(CruiseMissile.XmlActor.CollisionCenter, movement.LocalMatrix);
// Pick the reference point we'll use to calculate new orientation and position.
Vector3 referencePosition = targetPosition;
Vector3 referenceVector = referencePosition - collisionCenter;
if (referenceVector == Vector3.Zero)
{
// We're at the target, just bail otherwise we end up with NaNs.
return;
}
Vector3 referenceVectorUnit = Vector3.Normalize(referenceVector);
Vector2 referenceVectorUnit2D = new Vector2(referenceVectorUnit.X, referenceVectorUnit.Y);
if (referenceVectorUnit2D.LengthSquared() > 0.00001f)
{
referenceVectorUnit2D.Normalize();
}
float lookAheadTime = Homing ? 0.7f : 0.33f;
float lookAheadDistance = lookAheadTime * Speed;
float distanceToTarg2D = new Vector2(referenceVector.X, referenceVector.Y).Length();
bool targetPastLookAhead = true;
if (lookAheadDistance > distanceToTarg2D)
{
lookAheadDistance = distanceToTarg2D;
targetPastLookAhead = false;
}
// If flying nap-of-the-earth, avoid upcoming terrain changes unless
// the target is closer than the look ahead distance.
if (TerrainFollowing)
{
Vector3 lookAheadPoint = collisionCenter
+ new Vector3(referenceVectorUnit2D, 0) * lookAheadDistance;
lookAheadPoint.Z = targetPosition.Z;
///
Terrain.HitBlock hitBlock = new Boku.SimWorld.Terra.Terrain.HitBlock();
Vector2 minMaxZ = new Vector2(-1.0f, Single.MaxValue);
Vector4 maxStep = new Vector4(
float.MaxValue, // max single step up
float.MinValue, // max step down
-1.0f, // water depth at which transition land to water occurs (-1 to ignore)
-1.0f); // water depth at which transition water to land occurs (-1 to ignore)
SimWorld.Terra.Terrain.Blocked(
collisionCenter,
lookAheadPoint,
minMaxZ,
maxStep,
ref hitBlock,
missile.Movement.Altitude);
float maxHeightAhead = hitBlock.Max;
if (maxHeightAhead > 0)
{
lookAheadPoint.Z = maxHeightAhead + EditHeight;
if (Homing)
{
lookAheadPoint.Z = Math.Max(lookAheadPoint.Z, targetPosition.Z);
}
checkedForLand = false;
}
else
{
/// Check if any more terrain is coming up. If not, we'll
/// early terminate.
CheckForMoreLand(collisionCenter, referenceVectorUnit2D);
}
// Pick the best height value
float waterHeight = Terrain.GetWaterHeight(lookAheadPoint);
if (StayOverWater)
{
lookAheadPoint.Z = Math.Max(lookAheadPoint.Z, waterHeight + EditHeight);
maxHeightAhead = Math.Max(maxHeightAhead, waterHeight);
}
if (waterHeight > 0)
{
CheckRipples(thing, collisionCenter, CruiseMissile.XmlActor.CollisionRadius, waterHeight);
}
// Update the look-ahead vector with the new height
Vector3 lookAheadVectorUnit = new Vector3(
referenceVectorUnit2D, lookAheadPoint.Z - collisionCenter.Z);
lookAheadVectorUnit.Normalize();
/// We need to pitch up to follow the terrain if:
/// a) We aren't homing
/// b) We are homing but:
/// i) We are still far off from our target
/// ii) We are close but would smack into the terrain if we
/// bee-line toward it.
bool adjustPitch = !Homing ||
targetPastLookAhead ||
(referencePosition.Z < maxHeightAhead);
if (adjustPitch)
{
// Use the look-ahead vector as our desired flight direction.
referenceVector = lookAheadVectorUnit * 50f;
referencePosition = collisionCenter + referenceVector;
referenceVectorUnit = lookAheadVectorUnit;
}
}
// Update yaw
float deltaTurn = 0;
if (referenceVectorUnit2D.LengthSquared() > 0.00001f)
{
float desiredTurn = (float)(Math.Acos(referenceVectorUnit2D.X) * Math.Sign(referenceVectorUnit2D.Y));
deltaTurn = GetShorterAngle(desiredTurn - GetShorterAngle(turnAngle));
deltaTurn = MathHelper.Clamp(deltaTurn, -MaxRotationRate * secs, MaxRotationRate * secs);
turnAngle += deltaTurn;
}
// Update roll
rollAngle = MyMath.Lerp(rollAngle, -deltaTurn / MaxRotationRate / secs, secs * 10f);
// Update pitch
float desiredPitch = (float)(Math.Atan2(referenceVectorUnit.Z, new Vector2(referenceVectorUnit.X, referenceVectorUnit.Y).Length()));
float deltaPitch = GetShorterAngle(desiredPitch - GetShorterAngle(pitchAngle));
if (Speed > 0)
{
float posMaxPitch = Homing ? MaxPitchRate * 2.0f : MaxPitchRate * 5.0f;
float negMaxPitch = Homing ? MaxPitchRate * 0.5f : MaxPitchRate * 0.5f;
/// Speed of 0 means this is our first frame and we haven't accelerated up yet.
/// Go ahead and start off pitched toward the right direction.
float maxPitch = ((pitchAngle < 0) && (deltaPitch < 0) ? negMaxPitch : posMaxPitch);
maxPitch *= secs;
deltaPitch = MathHelper.Clamp(deltaPitch, -maxPitch, maxPitch);
}
pitchAngle += deltaPitch;
// Build our new local rotation matrix
Matrix rotation = Matrix.CreateFromQuaternion(
Quaternion.CreateFromAxisAngle(Vector3.Backward, turnAngle) *
Quaternion.CreateFromAxisAngle(Vector3.Down, pitchAngle) *
Quaternion.CreateFromAxisAngle(Vector3.Right, rollAngle));
// Find our new position along our rotated forward axis.
Vector3 velocity = rotation.Right * Speed;
Vector3 position = movement.Position + velocity * secs;
// Update the velocity. This isn't currently used by the chassis
// but is needed for the collision testing to work correctly.
movement.Velocity = velocity;
// Set our new rotation and translation.
movement.LocalMatrix = rotation * Matrix.CreateTranslation(position);
// Adjust from our actual to desired speed.
if (Speed < DesiredSpeed)
{
Speed = MyMath.Clamp <float>(Speed + secs * 10f, Speed, DesiredSpeed);
}
else if (Speed > DesiredSpeed)
{
Speed = MyMath.Clamp <float>(Speed - secs * 2f, DesiredSpeed, Speed);
}
}