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