private void Update()
{
if (mode == ScanningMode.Continuous)
{
float direction = clockwise ? 1 : -1;
scanningObject.Rotate(0, direction * Time.deltaTime * scanningSpeed, 0);
}
else
{
Vector3 currentOrientation = MathHelpers.Azimuthal(scanningObject.transform.forward).normalized;
float sinAngle = MathHelpers.CrossY(currentOrientation, m_targetOrientation);
if (Mathf.Abs(sinAngle) > m_sinMaxErrorDegrees)
{
// Move toward target orientation
float direction = Mathf.Sign(sinAngle);
scanningObject.Rotate(0, direction * Time.deltaTime * scanningSpeed, 0);
}
else if (!m_lingering)
{
// Target orientation reached; linger here a while
m_lingering = true;
m_lingerStartTime = Time.time;
}
else if (m_lingering && (Time.time - m_lingerStartTime) >= lingerTime)
{
// Select new target orientation
m_lingering = false;
m_targetOrientation = MathHelpers.RandomAzimuth();
}
}
}
private float HeadingErrorTo(Vector3 lookAtPoint)
{
Vector3 targetForward = lookAtPoint - transform.position;
Vector3 target = MathHelpers.Azimuthal(targetForward);
Vector3 forward = MathHelpers.Azimuthal(transform.forward);
return(Mathf.Sign(-MathHelpers.CrossY(forward, target)) * Vector3.Angle(forward, target));
}
private float ComputeYawAngleToTarget(Vector3 targetPos)
{
// gunYaw's parent object is used to determine its resting pose (angle=0).
// Target is rotated into parent's local coordinate system and the angle to
// the target in the azimuthal (xz) plane is the required yaw.
Vector3 localPoint = gunYaw.parent.InverseTransformPoint(targetPos);
Vector3 toTarget = MathHelpers.Azimuthal(localPoint - gunYaw.localPosition);
float direction = Mathf.Sign(MathHelpers.CrossY(Vector3.forward, toTarget));
return(direction * Vector3.Angle(Vector3.forward, toTarget));
}
private IEnumerator LookFlightDirectionCoroutine()
{
Rigidbody rb = GetComponent <Rigidbody>();
while (true)
{
Vector3 directionOfTravel = MathHelpers.Azimuthal(rb.velocity);
LookAt(transform.position + directionOfTravel);
UpdateControls();
yield return(null);
}
}
private void FixedUpdate()
{
if (m_target == null)
{
return;
}
float distance = MathHelpers.Azimuthal(transform.position - m_target.position).magnitude;
if (distance < startFollowDistance)
{
DisableNavigationBehaviorsExcept(follow);
follow.enabled = true;
}
else if (distance > stopFollowDistance && follow.enabled == true)
{
switch (stopFollowBehavior)
{
case StopFollowBehavior.StayPut:
default:
DisableNavigationBehaviorsExcept();
break;
case StopFollowBehavior.ReturnToHome:
DisableNavigationBehaviorsExcept(moveTo);
moveTo.enabled = true;
moveTo.Move(m_homePosition,
() =>
{
DisableAllBehaviors();
scan.enabled = true;
});
break;
case StopFollowBehavior.Patrol:
DisableNavigationBehaviorsExcept(patrol);
patrol.enabled = true;
break;
}
}
if (distance < startTrackDistance)
{
track.enabled = true;
scan.enabled = false;
}
else if (distance > stopTrackDistance)
{
track.enabled = false;
scan.enabled = true;
}
}
private void FixedUpdate()
{
if (m_target == null)
{
return;
}
float distance = MathHelpers.Azimuthal(transform.position - m_target.position).magnitude;
if (distance < startFollowingDistance)
{
DisableNavigationBehaviorsExcept(follow);
follow.enabled = true;
}
else if (distance > stopFollowingDistance && follow.enabled == true)
{
// Return back to starting position and resume default behavior
DisableNavigationBehaviorsExcept();
moveTo.enabled = true;
moveTo.Move(m_homePosition, () => { EnableDefaultNavigationBehavior(); });
}
if (distance < startTrackDistance)
{
DisableTurretBehaviorsExcept(track);
track.enabled = true;
track.OnLockObtained =
() =>
{
m_lockedOnTarget = true;
fire.enabled = true;
};
track.OnLockLost =
() =>
{
m_lockedOnTarget = false;
fire.enabled = false;
};
}
else if (distance > stopTrackDistance && track.enabled == true)
{
// Reset turret, stop firing, and resume default behavior
DisableTurretBehaviorsExcept();
resetTurret.enabled = true;
resetTurret.OnComplete = () => { EnableDefaultTurretBehavior(); };
fire.enabled = false;
m_lockedOnTarget = false;
}
}
private bool TryAttackPatternStrafe(float altitude, System.Action OnComplete)
{
Vector3 position = transform.position;
position.y = altitude;
Vector3 right = MathHelpers.Azimuthal(transform.right);
float d1 = FindClearance(transform.position, right, 2) - 2 * m_boundingRadius;
float d2 = FindClearance(transform.position, -right, 2) - 2 * m_boundingRadius;
Vector3[] waypoints = new Vector3[2];
int firstIdx = Random.Range(0, 2) == 0 ? 0 : 1;
waypoints[firstIdx ^ 0] = position + right * d1;
waypoints[firstIdx ^ 1] = position - right * d2;
m_autopilot.FollowPathAndLookAt(waypoints, m_target, OnComplete);
DrawLine(waypoints);
return(true);
}
private IEnumerator FollowCoroutine(Transform target, float distance, System.Action OnComplete)
{
// Follow at same altitude and maintain distance until timeout
float startTime = Time.time;
while (Time.time - startTime < timeout)
{
Vector3 position = target.position + MathHelpers.Azimuthal(transform.position - target.position).normalized *distance;
GoTo(position);
UpdateControls();
yield return(null);
}
if (OnComplete != null)
{
yield return(null); // always ensure one frame elapsed before callback
OnComplete();
}
Halt();
}
private bool TryBackOffPattern(Vector3 toTarget)
{
//TODO: margin represents, roughly, the diameter of a bounding sphere
// around us. This should probably be computed at start up from the
// collider footprint.
float margin = 0.75f;
// Measure clearance behind us
Vector3 back = -MathHelpers.Azimuthal(toTarget);
Ray ray = new Ray(transform.position, back);
RaycastHit hit;
float clearance = 0;
if (Physics.Raycast(ray, out hit, 10f))
{
clearance = (hit.point - transform.position).magnitude;
}
else
{
clearance = 10;
}
// Is there enough?
if (clearance - margin <= 0)
{
return(false);
}
// If so, move back some random distance but at least halfway to maximum
float minDistance = 0.5f * (margin + (clearance - margin));
float maxDistance = clearance - margin;
float distance = UnityEngine.Random.Range(minDistance, maxDistance);
m_autopilot.FlyTo(transform.position + distance * back, m_target);
DrawLine(new Vector3[] { transform.position, transform.position + distance * back });
return(true);
}
private void Update()
{
if (target == null)
{
return;
}
bool hasVerticalObject = false;
bool azimuthalLock = false;
bool azimuthalPerfectLock = false;
bool verticalLock = false;
bool verticalPerfectLock = false;
Vector3 targetPosition = target.position;
if (azimuthalTrackingObject != null)
{
Vector3 targetLocalPosition = MathHelpers.Azimuthal(azimuthalTrackingObject.transform.InverseTransformPoint(targetPosition));
float sinAngle = MathHelpers.CrossY(Vector3.forward, targetLocalPosition.normalized); // only the y component will be valid and we want it signed
float absSinAngle = Mathf.Abs(sinAngle);
if (absSinAngle > m_sinMaxErrorDegrees)
{
float direction = Mathf.Sign(sinAngle);
azimuthalTrackingObject.Rotate(0, direction * Time.deltaTime * azimuthalSpeed, 0);
azimuthalLock = absSinAngle <= m_sinLockDegrees;
}
else
{
azimuthalLock = true;
azimuthalPerfectLock = true;
}
}
if (verticalTrackingObject != null && azimuthalTrackingObject != null)
{
hasVerticalObject = true;
// Transform both the target and the vertical rotating object into the
// local coordinate system of the azimuthal object, whose xz-plane will
// be our ground plane from which to measure vertical angle.
Vector3 targetLocalVector = azimuthalTrackingObject.transform.InverseTransformPoint(targetPosition);
Vector3 objectLocalVector = azimuthalTrackingObject.transform.InverseTransformVector(verticalTrackingObject.forward);
// Compute the angles from the ground (or rather, the sine of the angles)
float sinTargetAngle = targetLocalVector.y / targetLocalVector.magnitude;
float sinObjectAngle = objectLocalVector.y / objectLocalVector.magnitude;
// Clamp the target angle to allowable range
sinTargetAngle = Mathf.Clamp(sinTargetAngle, m_sinMinVerticalAngle, m_sinMaxVerticalAngle);
// Rotate appropriately to minimize error
float deltaSinAngle = sinObjectAngle - sinTargetAngle;
float absDeltaSinAngle = Mathf.Abs(deltaSinAngle);
if (absDeltaSinAngle > m_deltaSinMaxErrorDegrees)
{
float direction = Mathf.Sign(deltaSinAngle);
verticalTrackingObject.Rotate(direction * Time.deltaTime * verticalSpeed, 0, 0);
verticalLock = absDeltaSinAngle <= m_sinLockDegrees;
}
else
{
verticalLock = true;
verticalPerfectLock = true;
}
/*
* // This code is a reference implementation that computes everything in
* // angles but requires the use of slow arcsin functions
* Vector3 targetLocalPosition = azimuthalTrackingObject.transform.InverseTransformPoint(targetPosition);
* Vector3 objectLocalPosition = azimuthalTrackingObject.transform.InverseTransformVector(verticalTrackingObject.forward);
* float deltaAngle = Mathf.Rad2Deg * (Mathf.Asin(targetLocalPosition.y / targetLocalPosition.magnitude) - Mathf.Asin(objectLocalPosition.y / objectLocalPosition.magnitude));
* if (Mathf.Abs(deltaAngle ) > maxErrorDegrees)
* {
* float direction = -Mathf.Sign(deltaAngle);
* verticalTrackingObject.Rotate(direction * Time.deltaTime * verticalSpeed, 0, 0);
* }
*/
}
// Callbacks
bool oldLockState = lockedOn;
lockedOn = false;
if (!hasVerticalObject && azimuthalLock)
{
lockedOn = true;
perfectLock = azimuthalPerfectLock;
if (OnLockObtained != null && oldLockState == false)
{
OnLockObtained();
}
}
else if (verticalLock && azimuthalLock)
{
lockedOn = true;
perfectLock = azimuthalPerfectLock && verticalPerfectLock;
if (OnLockObtained != null && oldLockState == false)
{
OnLockObtained();
}
}
if (OnLockLost != null && oldLockState == true && lockedOn == false)
{
OnLockLost();
}
}
private IEnumerator OrbitPositionCoroutine(UpdateVectorCallback GetOrbitCenter, UpdateScalarCallback GetOrbitAltitude, float orbitRadius, float timeout, System.Action OnComplete)
{
float step = 20;
float direction = MathHelpers.RandomSign();
Vector3 toHelicopter = MathHelpers.Azimuthal(transform.position - GetOrbitCenter(Time.deltaTime));
float startRadius = toHelicopter.magnitude;
float currentRadius = startRadius;
float startAngle = currentRadius == 0 ? 0 : Mathf.Rad2Deg * Mathf.Acos(toHelicopter.x / currentRadius);
if (startAngle < 0)
{
startAngle += 180;
}
float currentAngle = startAngle;
float degreesElapsed = 0;
float startAltitude = transform.position.y;
float currentAltitude = startAltitude;
int maxObstructionRetries = (int)(360 / step);
int numObstructionRetries = maxObstructionRetries;
float nextObstructionCheckTime = 0;
float timeoutTime = Time.time + timeout;
while (true)
{
// Compute the next position along the circle
float nextAngle = currentAngle + direction * step;
float nextRadians = Mathf.Deg2Rad * nextAngle;
// Move to that position
bool flying = true;
do
{
float now = Time.time;
if (now >= timeoutTime)
{
if (OnComplete != null)
{
yield return(null); // always ensure one frame elapsed before callback
OnComplete();
}
Halt();
yield break;
}
// In case orbit center is a moving target, continually update the next
// position
Vector3 nextPosition = GetOrbitCenter(Time.deltaTime) + currentRadius * new Vector3(Mathf.Cos(nextRadians), 0, Mathf.Sin(nextRadians));
nextPosition.y = currentAltitude;
// This doesn't work very well but the idea is to check for
// obstructions and move on to subsequent waypoints. If no point seems
// reachable, we simply abort. For each attempt, we allow some time to
// pass in case we were stuck.
if (now > nextObstructionCheckTime && PathObstructed(nextPosition))
{
if (numObstructionRetries > 0)
{
nextObstructionCheckTime = now + obstructionRetryTime;
numObstructionRetries--;
break;
}
// Need to abort
if (OnComplete != null)
{
yield return(null); // always ensure one frame elapsed before callback
OnComplete();
}
Halt();
yield break;
}
flying = GoTo(nextPosition);
UpdateControls();
yield return(null);
} while (flying);
// Restore number of avoidance attempts if we finally reached a waypoint
if (flying == false)
{
numObstructionRetries = maxObstructionRetries;
}
// Advance the angle
degreesElapsed += step;
currentAngle = nextAngle;
float revolutionCompleted = degreesElapsed / 360;
// Adjust the radius so that it converges to the desired radius within
// one revolution
currentRadius = Mathf.Lerp(startRadius, orbitRadius, revolutionCompleted);
// Altitude convergence
currentAltitude = Mathf.Lerp(startAltitude, GetOrbitAltitude(Time.deltaTime), revolutionCompleted);
}
}
private bool TooFarFromTarget()
{
return(MathHelpers.Azimuthal(m_agent.destination - target.position).magnitude > targetDistance);
}
private bool TryAttackPatternVerticalAndBehind(Vector3 toTarget, Vector3 verticalDirection, System.Action OnComplete)
{
float minDistanceVertical = 2 * m_boundingRadius; // minimum distance above/beneath target that must be clear
float minDistanceBehind = 2 * m_boundingRadius; // minimum distance behind the target that must be clear
Vector3[] positions = new Vector3[2];
// Do we have enough clearance directly above/below to fly through?
float clearance = FindClearance(m_target.position, verticalDirection);
if (clearance < minDistanceVertical)
{
Debug.Log("VERTICAL: NO CLEARANCE: " + clearance);
return(false);
}
// Choose a point halfway between the minimum vertical distance and
// clearance
float mid = 0.5f * (minDistanceVertical + clearance);
positions[0] = m_target.position + verticalDirection * mid;
if (IsPathBlocked(positions[0]))
{
Debug.Log("VERTICAL: PATH BLOCKED");
return(false);
}
// Perform raycasts in a semicircle behind (and at same altitude as) target
Vector3 directionBehind = MathHelpers.Azimuthal(toTarget).normalized;
Vector3 bestDirection = Vector3.zero;
float bestClearance = 0;
for (float angle = -90; angle < 90; angle += 20)
{
Vector3 direction = Quaternion.Euler(angle * Vector3.up) * directionBehind;
clearance = FindClearance(m_target.position, direction);
if (clearance > bestClearance)
{
bestDirection = direction;
bestClearance = clearance;
}
}
// Check if sufficient room to go behind
if (bestClearance < minDistanceBehind)
{
Debug.Log("REAR: NO CLEARANCE: " + bestClearance);
return(false);
}
// Determine point, ideally in between the min and max distance
mid = 0.5f * (minDistanceBehind + bestClearance);
positions[1] = m_target.position + bestDirection * mid;
if (IsPathBlocked(positions[1]))
{
Debug.Log("REAR: PATH BLOCKED");
return(false);
}
Debug.Log("LAUNCHING ATTACK PATTERN");
DrawLine(new Vector3[] { transform.position, positions[0], positions[1] });
m_autopilot.FollowPathAndLookAt(positions, m_target, OnComplete);
return(true);
}
private void UpdateDynamics()
{
/*
* Axis convention
* ---------------
*
* Forward = blue = +z
* Right = red = +x
* Up = green = +y
*
* Linear terminal velocity
* ------------------------
*
* Velocity should be integrated like this:
*
* A = F/m
* Vf = Vi + (A - drag*Vi)*dt
*
* Solving for Vmax = Vi = Vf: Vmax = A/drag
* But unfortunately, Unity implements drag incorrectly as:
*
* Vt = Vi + A*dt
* Vf = Vt - Vt*drag*dt
*
* Or:
*
* Vf = (Vi + A*dt) * (1-drag*dt)
*
* Which gives a different terminal velocity:
*
* Vmax = A/drag - A*dt = A*(1/drag - dt)
*
* Translational xz control
* ------------------------
*
* Translational motion is in xz plane and oriented relative to the
* helicopter's heading (forward vector projected onto xz plane). The input
* is in units of g-force (-1 to 1 g).
*
* Pitch and roll proportional to input strength (and therefore speed) are
* induced, up to a maximum angle, simulating how a helicopter looks in
* flight without requiring accurate modeling of rotor force and torques.
*
* Pitch and roll angles are relative to the xz plane and are computed by
* projecting local forward and right onto the plane. Cannot use Euler
* angles because Euler rotations are applied sequentially and each depends
* on the previous.
*
* Torques are applied to induce the desired orientation change, with
* magnitude proportional to angular error (i.e., a P controller).
*
* Rotational control
* ------------------
*
* Rotational input rotates around *local* up axis (yaw) by applying a
* torque. Torque is a multiple of torque used to induce pitch and roll to
* provide a faster response time because user may want to quickly rotate
* full 360 degrees.
*
* Altitude control
* ----------------
*
* Altitude input is relative to rotor force (along local up) needed to
* maintain constant altitude. Input of 0 hovers, -1 is free fall.
*
* Note: To hover, we need to control y component of rotor force. When
* helicopter is angled, the rotor force has to be larger, which also adds
* additional translational force beyond the translational inputs, e.g.
* causing the helicopter to travel faster forwards as it climbs.
*
* Note on torque calculation
* --------------------------
* This might be completely wrong -- I need to review my basic physics :) I
* gleaned this from a cursory glance at the PhysX docs while trying to
* convert torque values I had specified as actual torques (N*m) into
* angular acceleration values suitable for use with ForceMode.Acceleration.
*
* When adding torque in the default force mode (in units of N*m), the
* value is multiplied by the inverse inertia in order to get acceleration.
* Therfore, the adjustment from torque -> acceleration here multiplies by
* the moment of inertia of a box: (m/12) * (x^2 + y^2), where x and y are
* the dimensions along the two axes perpendicular to the rotation axis.
*
* For yaw, given a torque, it seems we can eliminate mass by converting to
* acceleration using this approximate formula:
*
* Acceleration = Torque / Inertia
* Inertia = (Mass / 12) * (Width^2 + Depth^2)
*
* The masses can then by canceled from torque and inertia.
*/
Controls controls = m_controls;
Vector3 torque = Vector3.zero;
// Translational motion, rotated into absolute (world) coordinate space
float currentHeadingDeg = heading;
Vector3 translationalInput = new Vector3(controls.lateral, 0, controls.longitudinal);
Vector3 translationalForce = Quaternion.Euler(new Vector3(0, currentHeadingDeg, 0)) * translationalInput * TRANSLATIONAL_ACCELERATION;
// Pitch, roll, and yaw
float targetPitchDeg = Mathf.Sign(controls.longitudinal) * Mathf.Lerp(0, MAX_TILT_DEGREES, Mathf.Abs(controls.longitudinal));
float currentPitchDeg = Mathf.Sign(-transform.forward.y) * Vector3.Angle(transform.forward, MathHelpers.Azimuthal(transform.forward));
float targetRollDeg = Mathf.Sign(-controls.lateral) * Mathf.Lerp(0, MAX_TILT_DEGREES, Mathf.Abs(controls.lateral));
float currentRollDeg = Mathf.Sign(transform.right.y) * Vector3.Angle(transform.right, MathHelpers.Azimuthal(transform.right));
float pitchErrorDeg = targetPitchDeg - currentPitchDeg;
float rollErrorDeg = targetRollDeg - currentRollDeg;
torque += PITCH_ROLL_CORRECTIVE_TORQUE * new Vector3(pitchErrorDeg, 0, rollErrorDeg);
torque += YAW_CORRECTIVE_TORQUE * controls.rotational * Vector3.up;
// Acceleration from force exerted by rotors
float hoverAcceleration = Mathf.Abs(Physics.gravity.y);
float rotorAcceleration = ALTITUDE_ACCELERATION * controls.altitude;
// Apply all forces
m_rb.AddRelativeForce(Vector3.up * rotorAcceleration, ForceMode.Acceleration);
m_rb.AddForce(Vector3.up * hoverAcceleration, ForceMode.Acceleration);
m_rb.AddForce(translationalForce, ForceMode.Acceleration);
m_rb.AddRelativeTorque(torque, ForceMode.Acceleration);
// Pitch of engine sound is based on tilt, which is based on desired speed
//float engineOutput = Mathf.Max(Mathf.Abs(controls.longitudinal), Mathf.Abs(controls.lateral));
//m_rotorAudioSource.pitch = Mathf.Lerp(1.0f, 1.3f, engineOutput);
// Rotor speed
float mainRevolutionsPerSecond = Mathf.Lerp(2.5f, 6, Mathf.Clamp01(controls.EngineMagnitude()));
float tailRevolutionsPerSecond = Mathf.Lerp(2.5f, 6, (Mathf.Clamp(controls.rotational, -1, 1) + 1) * 0.5f);
if (mainRotor != null)
{
mainRotor.angularVelocity = mainRevolutionsPerSecond * 360;
}
if (tailRotor != null)
{
tailRotor.angularVelocity = tailRevolutionsPerSecond * 360;
}
}
