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