public static Vector3 ClampBend(Vector3 vector, Vector3 reference, float maxBendAngle) { float vLenSqr = vector.sqrMagnitude; if (vLenSqr < MathUtil.Epsilon) { return(vector); } float rLenSqr = reference.sqrMagnitude; if (rLenSqr < MathUtil.Epsilon) { return(vector); } Vector3 vUnit = vector / Mathf.Sqrt(vLenSqr); Vector3 rUnit = reference / Mathf.Sqrt(rLenSqr); Vector3 cross = Vector3.Cross(rUnit, vUnit); float dot = Vector3.Dot(rUnit, vUnit); Vector3 axis = cross.sqrMagnitude > MathUtil.Epsilon ? cross.normalized : FindOrthogonal(rUnit); float angle = Mathf.Acos(Mathf.Clamp01(dot)); if (angle <= maxBendAngle) { return(vector); } Quaternion clampedBendRot = QuaternionUtil.AxisAngle(axis, maxBendAngle); Vector3 result = clampedBendRot * reference; result *= Mathf.Sqrt(vLenSqr) / Mathf.Sqrt(rLenSqr); return(result); }
void FixedUpdate() { float dt = Time.fixedDeltaTime; Vector3 linearInputVec = Vector3.zero; if (Input.GetKey(KeyCode.W)) { linearInputVec += Vector3.forward; } if (Input.GetKey(KeyCode.S)) { linearInputVec += Vector3.back; } if (Input.GetKey(KeyCode.A)) { linearInputVec += Vector3.left; } if (Input.GetKey(KeyCode.D)) { linearInputVec += Vector3.right; } if (Input.GetKey(KeyCode.R)) { linearInputVec += Vector3.up; } if (Input.GetKey(KeyCode.F)) { linearInputVec += Vector3.down; } bool linearThrustOn = linearInputVec.sqrMagnitude > MathUtil.Epsilon; if (linearThrustOn) { linearInputVec = linearInputVec.normalized * LinearThrust; m_linearVelocity += linearInputVec * dt; m_linearVelocity = VectorUtil.ClampLength(m_linearVelocity, 0.0f, MaxLinearSpeed); } else { m_linearVelocity = VectorUtil.ClampLength(m_linearVelocity, 0.0f, Mathf.Max(0.0f, m_linearVelocity.magnitude - LinearDrag * dt)); } float speed = m_linearVelocity.magnitude; float tSpeed = speed * MathUtil.InvSafe(MaxLinearSpeed); Quaternion tiltRot = Quaternion.identity; float tHorizontal = 1.0f; float tHorizontalSpeed = 0.0f; if (speed > MathUtil.Epsilon) { Vector3 flatVel = m_linearVelocity; flatVel.y = 0.0f; tHorizontal = m_linearVelocity.magnitude > 0.01f ? 1.0f - Mathf.Clamp01(Mathf.Abs(m_linearVelocity.y) / m_linearVelocity.magnitude) : 0.0f; tHorizontalSpeed = Mathf.Min(1.0f, speed / Mathf.Max(MathUtil.Epsilon, MaxLinearSpeed)) * tHorizontal; Vector3 tiltAxis = Vector3.Cross(Vector3.up, flatVel).normalized; float tiltAngle = Tilt * MathUtil.Deg2Rad * tHorizontalSpeed; tiltRot = QuaternionUtil.AxisAngle(tiltAxis, tiltAngle); } float angularInput = 0.0f; if (Input.GetKey(KeyCode.Q)) { angularInput -= 1.0f; } if (Input.GetKey(KeyCode.E)) { angularInput += 1.0f; } bool largerMaxAngularSpeed = Input.GetKey(KeyCode.LeftControl); bool angularThurstOn = Mathf.Abs(angularInput) > MathUtil.Epsilon; if (angularThurstOn) { float maxAngularSpeed = MaxAngularSpeed * (largerMaxAngularSpeed ? 2.5f : 1.0f); angularInput *= AngularThrust * MathUtil.Deg2Rad; m_angularVelocity += angularInput * dt; m_angularVelocity = Mathf.Clamp(m_angularVelocity, -maxAngularSpeed * MathUtil.Deg2Rad, maxAngularSpeed * MathUtil.Deg2Rad); } else { m_angularVelocity -= Mathf.Sign(m_angularVelocity) * Mathf.Min(Mathf.Abs(m_angularVelocity), AngularDrag * MathUtil.Deg2Rad * dt); } m_yawAngle += m_angularVelocity * dt; Quaternion yawRot = QuaternionUtil.AxisAngle(Vector3.up, m_yawAngle); m_hoverCenter += m_linearVelocity * dt; m_hoverPhase += Time.deltaTime; Vector3 hoverVec = 0.05f * Mathf.Sin(1.37f * m_hoverPhase) * Vector3.right + 0.05f * Mathf.Sin(1.93f * m_hoverPhase + 1.234f) * Vector3.forward + 0.04f * Mathf.Sin(0.97f * m_hoverPhase + 4.321f) * Vector3.up; hoverVec *= Hover; Quaternion hoverQuat = Quaternion.FromToRotation(Vector3.up, hoverVec + Vector3.up); transform.position = m_hoverCenter + hoverVec; transform.rotation = tiltRot * yawRot * hoverQuat; if (Motor != null) { float motorAngularSpeedDeg = Mathf.Lerp(MotorBaseAngularSpeed, MotorMaxAngularSpeed, tHorizontalSpeed); m_motorAngle += motorAngularSpeedDeg * MathUtil.Deg2Rad * dt; Motor.localRotation = QuaternionUtil.AxisAngle(Vector3.up, m_motorAngle - m_yawAngle); } if (BubbleEmitter != null) { var emission = BubbleEmitter.emission; emission.rateOverTime = Mathf.Lerp(BubbleBaseEmissionRate, BubbleMaxEmissionRate, tSpeed); } if (Eyes != null) { m_blinkTimer -= dt; if (m_blinkTimer <= 0.0f) { bool doubleBlink = !m_lastBlinkWasDouble && Random.Range(0.0f, 1.0f) > 0.75f; m_blinkTimer = doubleBlink ? 0.2f : BlinkInterval + Random.Range(1.0f, 2.0f); m_lastBlinkWasDouble = doubleBlink; m_eyeScaleSpring.Value.y = 0.0f; m_eyePositionLsSpring.Value.y -= 0.025f; } Eyes.localScale = m_eyeScaleSpring.TrackDampingRatio(m_eyeInitScale, 30.0f, 0.8f, dt); Eyes.localPosition = m_eyePositionLsSpring.TrackDampingRatio(m_eyeInitPositionLs, 30.0f, 0.8f, dt); } }