/* * * /// <summary> * /// Turn toward a world position in space. * /// </summary> * /// <param name="vehicleTransform">The transform of the vehicle.</param> * /// <param name="targetPosition">The position the vehicle must turn toward.</param> * /// <param name="PIDCoeffs">The PID coefficients for this vehicle.</param> * /// <param name="maxRotationAngles">The maximum rotation angles about each axis that the vehicle can reach during maneuvers.</param> * /// <param name="controlValues">Get the control values calculated for the maneuver.</param> * /// <param name="integralVals">Get the updated integral values for the PID controller.</param> * public static void MatchRotation(Transform vehicleTransform, Transform targetTransform, Vector3 PIDCoeffs, out Vector3 controlValues, ref Vector3 integralVals) * { * * Vector3 dir_fwd = targetTransform.forward; * Vector3 dir_up = targetTransform.up; * * Vector3 targetPosition = vehicleTransform.position + dir_fwd; * * Vector3 tmpRelPos = GetRollAndPitchUnaffectedRelPos(vehicleTransform, targetPosition); * float zxAngleGlobal = Vector3.Angle(Vector3.forward, new Vector3(tmpRelPos.x, 0f, tmpRelPos.z)); * if (tmpRelPos.y > 0) * { * zxAngleGlobal *= -1; * } * * // Get the roll angle * float xyAngleGlobal = Vector3.Angle(Vector3.up, new Vector3(tmpRelPos.x, tmpRelPos.y, 0)); * if (tmpRelPos.x > 0) * { * xyAngleGlobal *= -1; * } * * // Get the ship's current roll * float currentRollAngle = GetRollAngleFromHorizon(vehicleTransform); * * float desiredRollAngle = GetRollAngleFromHorizon(targetTransform); * * float relativeRollAngle = desiredRollAngle - currentRollAngle; * * Vector3 toTargetDir = (targetPosition - vehicleTransform.position).normalized; * * Vector3 tmp = GetRollAndPitchUnaffectedRelPos(vehicleTransform, targetPosition); * * // This is the pitch from the totarget vector to the vertically flattened totarget vector * float pitchAngleToTarget_Global = Vector3.Angle(toTargetDir, new Vector3(toTargetDir.x, 0, toTargetDir.z)); * pitchAngleToTarget_Global = tmp.y > 0 ? pitchAngleToTarget_Global * -1 : pitchAngleToTarget_Global; * * // this is the pitch from the ship's forward vector to the vertically flattened ships forward vector * float currentPitchAngle_Global = Vector3.Angle(vehicleTransform.forward, new Vector3(vehicleTransform.forward.x, 0f, vehicleTransform.forward.z)); * currentPitchAngle_Global = vehicleTransform.forward.y > 0 ? currentPitchAngle_Global * -1 : currentPitchAngle_Global; * float clampedTargetPitch = pitchAngleToTarget_Global; * * float relativePitchAngle = clampedTargetPitch - currentPitchAngle_Global; * * // THis is is yaw angle from the vertically flattened ship forward, to the vertically flattened totarget * float yawAngleToTarget_Global = Vector3.Angle(Vector3.forward, new Vector3(tmp.x, 0f, tmp.z)); * yawAngleToTarget_Global = tmp.x < 0 ? yawAngleToTarget_Global * -1 : yawAngleToTarget_Global; * * Vector3 pitchAndYaw = new Vector3(yawAngleToTarget_Global, relativePitchAngle, 0f); * pitchAndYaw = Quaternion.Euler(0f, 0f, currentRollAngle) * pitchAndYaw; * * Vector3 proportionalVals; * * proportionalVals.x = pitchAndYaw.y; * proportionalVals.y = pitchAndYaw.x; * proportionalVals.z = relativeRollAngle; * proportionalVals *= PIDCoeffs.x; * * // ****** PID coeffs ****** * * // Derivative (braking) torque * * Vector3 derivativeVals = -1f * PIDCoeffs.z * proportionalVals; * * // Integral * float angleToTarget = Vector3.Angle(vehicleTransform.forward, targetPosition - vehicleTransform.position); * if (angleToTarget > 25) * { * integralVals = Vector3.zero; * } * else * { * integralVals += PIDCoeffs.y * proportionalVals; * } * * integralVals.x = Mathf.Clamp(integralVals.x, -1f, 1f); * integralVals.z = Mathf.Clamp(integralVals.z, -1f, 1f); * integralVals.y = Mathf.Clamp(integralVals.y, -1f, 1f); * * // Calculate control values * controlValues = proportionalVals + derivativeVals + integralVals; * controlValues.z = proportionalVals.z; * * controlValues.x = Mathf.Clamp(controlValues.x, -1f, 1f); * controlValues.z = Mathf.Clamp(controlValues.z, -1f, 1f); * controlValues.y = Mathf.Clamp(controlValues.y, -1f, 1f); * * * } * * * /// <summary> * /// Do formation behaviour (using throttle to control forward/backward, rather than always turning toward target) * /// </summary> * /// <param name="vehicle">The vehicle that is maneuvring.</param> * /// <param name="formationLeader">The formation leader of the vehicle.</param> * /// <param name="offset">The offset from the formation leader that this vehicle must maintain.</param> * /// <param name="throttlePIDCoeffs">The PID controller coefficients for the throttle.</param> * /// <param name="integralThrottleVals">The integral valued for the throttle PID controller.</param> * /// <param name="throttleValues">Get the calculated throttle values.</param> * /// <returns>The world position steering target for this maneuver.</returns> * public static Vector3 Formation(Rigidbody rBody, Transform t, Transform formationLeader, Vector3 offset, ShipPIDController controller) * { * * // Get the relative target vector * Vector3 targetPos = formationLeader.position + formationLeader.TransformDirection(offset); * Vector3 targetRelPos = t.InverseTransformPoint(targetPos); * * float forwardProjection = Mathf.Clamp(1 - Vector3.Distance(t.position, targetPos)/200, 0f, 1f) * 100; * * Vector3 steeringTarget = formationLeader.position + formationLeader.TransformDirection(offset) + formationLeader.forward * forwardProjection; * * Vector3 closingVelocity = rBody.velocity - rBody.velocity; * float closingDirectionAmount = Vector3.Dot(closingVelocity.normalized, (steeringTarget - t.position).normalized); * * // calculate the throttle * float proportionalThrottle = targetRelPos.z * throttlePIDCoeffs.x; * * float derivativeThrottle = -1 * Mathf.Clamp(closingDirectionAmount, 0, 1) * closingVelocity.magnitude * throttlePIDCoeffs.z; * * float integralThrottle = throttlePIDCoeffs.y * proportionalThrottle; * integralThrottleVals.z += integralThrottle; * integralThrottleVals.z = Mathf.Clamp(integralThrottleVals.z, 0, 1); * * throttleValues = new Vector3(0f, 0f, Mathf.Clamp(proportionalThrottle + derivativeThrottle + integralThrottleVals.z, 0f, 1f)); * * return steeringTarget; * * } */ public static void TranslateToward(Rigidbody rBody, Vector3 targetPosition, PIDController3D movementPIDController) { // Get the relative target position Vector3 targetRelPos = rBody.transform.InverseTransformPoint(targetPosition); // Get the to-target vector Vector3 toTargetVector = targetPosition - rBody.transform.position; // Calculate the closing velocity Vector3 closingVelocity = Vector3.Dot(rBody.velocity.normalized, toTargetVector.normalized) * rBody.velocity; Vector3 distanceChangeRate = -1 * new Vector3(Mathf.Abs(closingVelocity.x), Mathf.Abs(closingVelocity.y), Mathf.Abs(closingVelocity.z)); // Update the PID Controller movementPIDController.SetError(PIDController3D.Axis.X, targetRelPos.x, distanceChangeRate.x); movementPIDController.SetError(PIDController3D.Axis.Y, targetRelPos.y, distanceChangeRate.y); movementPIDController.SetError(PIDController3D.Axis.Z, targetRelPos.z, distanceChangeRate.z); float integralPhaseInDistance = 100; float dist = targetRelPos.magnitude; float amount = Mathf.Clamp(1 - (dist / integralPhaseInDistance), 0, 1); movementPIDController.SetIntegralInfluence(amount); }
/// <summary> /// Turn toward a world position in space. /// </summary> /// <param name="vehicleTransform">The transform of the vehicle.</param> /// <param name="targetPosition">The position the vehicle must turn toward.</param> /// <param name="PIDCoeffs">The PID coefficients for this vehicle.</param> /// <param name="maxRotationAngles">The maximum rotation angles about each axis that the vehicle can reach during maneuvers.</param> /// <param name="controlValues">Get the control values calculated for the maneuver.</param> /// <param name="integralVals">Get the updated integral values for the PID controller.</param> public static void TurnToward(Transform vehicleTransform, Vector3 targetPosition, Vector3 maxRotationAngles, PIDController3D steeringPIDController) { Vector3 tmpRelPos = GetRollAndPitchUnaffectedRelPos(vehicleTransform, targetPosition); float zxAngleGlobal = Vector3.Angle(Vector3.forward, new Vector3(tmpRelPos.x, 0f, tmpRelPos.z)); if (tmpRelPos.y > 0) { zxAngleGlobal *= -1; } // Get the roll angle float xyAngleGlobal = Vector3.Angle(Vector3.up, new Vector3(tmpRelPos.x, tmpRelPos.y, 0)); if (tmpRelPos.x > 0) { xyAngleGlobal *= -1; } // Get the ship's current roll float currentRollAngle = GetRollAngleFromHorizon(vehicleTransform); float desiredRollAngle; if (Mathf.Abs(xyAngleGlobal) > maxRotationAngles.z) { desiredRollAngle = maxRotationAngles.z * Mathf.Sign(xyAngleGlobal); } else { desiredRollAngle = xyAngleGlobal; } desiredRollAngle *= Mathf.Clamp((Mathf.Abs(zxAngleGlobal) / 45f) - 0.05f, 0, 1); float relativeRollAngle = desiredRollAngle - currentRollAngle; Vector3 toTargetDir = (targetPosition - vehicleTransform.position).normalized; Vector3 tmp = GetRollAndPitchUnaffectedRelPos(vehicleTransform, targetPosition); // This is the pitch from the totarget vector to the vertically flattened totarget vector float pitchAngleToTarget_Global = Vector3.Angle(toTargetDir, new Vector3(toTargetDir.x, 0, toTargetDir.z)); pitchAngleToTarget_Global = tmp.y > 0 ? pitchAngleToTarget_Global * -1 : pitchAngleToTarget_Global; // this is the pitch from the ship's forward vector to the vertically flattened ships forward vector float currentPitchAngle_Global = Vector3.Angle(vehicleTransform.forward, new Vector3(vehicleTransform.forward.x, 0f, vehicleTransform.forward.z)); currentPitchAngle_Global = vehicleTransform.forward.y > 0 ? currentPitchAngle_Global * -1 : currentPitchAngle_Global; float clampedTargetPitch = Mathf.Clamp(pitchAngleToTarget_Global, -maxRotationAngles.x, maxRotationAngles.x); float relativePitchAngle = clampedTargetPitch - currentPitchAngle_Global; // THis is is yaw angle from the vertically flattened ship forward, to the vertically flattened totarget float yawAngleToTarget_Global = Vector3.Angle(Vector3.forward, new Vector3(tmp.x, 0f, tmp.z)); yawAngleToTarget_Global = tmp.x < 0 ? yawAngleToTarget_Global * -1 : yawAngleToTarget_Global; Vector3 pitchAndYaw = new Vector3(yawAngleToTarget_Global, relativePitchAngle, 0f); pitchAndYaw = Quaternion.Euler(0f, 0f, currentRollAngle) * pitchAndYaw; // Update the PID Controller steeringPIDController.SetError(PIDController3D.Axis.X, pitchAndYaw.y, -1f * pitchAndYaw.y); steeringPIDController.SetError(PIDController3D.Axis.Y, pitchAndYaw.x, -1f * pitchAndYaw.x); steeringPIDController.SetError(PIDController3D.Axis.Z, relativeRollAngle, -1f * relativeRollAngle); float angleToTarget = Vector3.Angle(vehicleTransform.forward, targetPosition - vehicleTransform.position); if (angleToTarget > 25) { steeringPIDController.SetIntegralInfluence(0); } else { steeringPIDController.SetIntegralInfluence(1); } }