예제 #1
0
        public Vector3 ComputeRequiredAngularAcceleration(Quaternion currentOrientation, Quaternion desiredOrientation, Vector3 currentAngularVelocity, float deltaTime)
        {
            Quaternion quaternion           = QuaternionExtensions.RequiredRotation(currentOrientation, desiredOrientation);
            Quaternion quaternion1          = Quaternion.identity.Subtract(quaternion);
            Quaternion eulerAngleQuaternion = PidQuaternionController.ToEulerAngleQuaternion(currentAngularVelocity) * quaternion;
            Matrix4x4  matrix4x4            = new Matrix4x4()
            {
                m00 = -quaternion.x * -quaternion.x + -quaternion.y * -quaternion.y + -quaternion.z * -quaternion.z,
                m01 = -quaternion.x * quaternion.w + -quaternion.y * -quaternion.z + -quaternion.z * quaternion.y,
                m02 = -quaternion.x * quaternion.z + -quaternion.y * quaternion.w + -quaternion.z * -quaternion.x,
                m03 = -quaternion.x * -quaternion.y + -quaternion.y * quaternion.x + -quaternion.z * quaternion.w,
                m10 = quaternion.w * -quaternion.x + -quaternion.z * -quaternion.y + quaternion.y * -quaternion.z,
                m11 = quaternion.w * quaternion.w + -quaternion.z * -quaternion.z + quaternion.y * quaternion.y,
                m12 = quaternion.w * quaternion.z + -quaternion.z * quaternion.w + quaternion.y * -quaternion.x,
                m13 = quaternion.w * -quaternion.y + -quaternion.z * quaternion.x + quaternion.y * quaternion.w,
                m20 = quaternion.z * -quaternion.x + quaternion.w * -quaternion.y + -quaternion.x * -quaternion.z,
                m21 = quaternion.z * quaternion.w + quaternion.w * -quaternion.z + -quaternion.x * quaternion.y,
                m22 = quaternion.z * quaternion.z + quaternion.w * quaternion.w + -quaternion.x * -quaternion.x,
                m23 = quaternion.z * -quaternion.y + quaternion.w * quaternion.x + -quaternion.x * quaternion.w,
                m30 = -quaternion.y * -quaternion.x + quaternion.x * -quaternion.y + quaternion.w * -quaternion.z,
                m31 = -quaternion.y * quaternion.w + quaternion.x * -quaternion.z + quaternion.w * quaternion.y,
                m32 = -quaternion.y * quaternion.z + quaternion.x * quaternion.w + quaternion.w * -quaternion.x,
                m33 = -quaternion.y * -quaternion.y + quaternion.x * quaternion.x + quaternion.w * quaternion.w
            };
            Quaternion quaternion2 = this.ComputeOutput(quaternion1, eulerAngleQuaternion, deltaTime);

            quaternion2 = PidQuaternionController.MultiplyAsVector(matrix4x4, quaternion2);
            Quaternion quaternion3 = quaternion2.Multiply(-2f) * Quaternion.Inverse(quaternion);

            return(new Vector3(quaternion3.x, quaternion3.y, quaternion3.z));
        }
        /// <summary>
        ///     Computes the angular acceleration required to rotate from the current orientation to
        ///     a desired orientation based on the specified current angular velocity for the current frame.
        /// </summary>
        /// <param name="currentOrientation">The current orientation.</param>
        /// <param name="desiredOrientation">The desired orientation.</param>
        /// <param name="currentAngularVelocity">The current angular velocity.</param>
        /// <param name="deltaTime">The frame delta time.</param>
        /// <returns>
        ///     The angular acceleration required to rotate from the current orientation to the desired orientation.
        /// </returns>
        public Vector3 ComputeRequiredAngularAcceleration(Quaternion currentOrientation, Quaternion desiredOrientation, Vector3 currentAngularVelocity, float deltaTime)
        {
            Quaternion requiredRotation = QuaternionExtensions.RequiredRotation(currentOrientation, desiredOrientation);

            Quaternion error           = Quaternion.identity.Subtract(requiredRotation);
            Quaternion angularVelocity = ToEulerAngleQuaternion(currentAngularVelocity);
            Quaternion delta           = angularVelocity * requiredRotation;

            var orthogonalizeMatrix = new Matrix4x4()
            {
                m00 =
                    -requiredRotation.x * -requiredRotation.x + -requiredRotation.y * -requiredRotation.y +
                    -requiredRotation.z * -requiredRotation.z,
                m01 =
                    -requiredRotation.x * requiredRotation.w + -requiredRotation.y * -requiredRotation.z +
                    -requiredRotation.z * requiredRotation.y,
                m02 =
                    -requiredRotation.x * requiredRotation.z + -requiredRotation.y * requiredRotation.w +
                    -requiredRotation.z * -requiredRotation.x,
                m03 =
                    -requiredRotation.x * -requiredRotation.y + -requiredRotation.y * requiredRotation.x +
                    -requiredRotation.z * requiredRotation.w,
                m10 =
                    requiredRotation.w * -requiredRotation.x + -requiredRotation.z * -requiredRotation.y +
                    requiredRotation.y * -requiredRotation.z,
                m11 =
                    requiredRotation.w * requiredRotation.w + -requiredRotation.z * -requiredRotation.z +
                    requiredRotation.y * requiredRotation.y,
                m12 =
                    requiredRotation.w * requiredRotation.z + -requiredRotation.z * requiredRotation.w +
                    requiredRotation.y * -requiredRotation.x,
                m13 =
                    requiredRotation.w * -requiredRotation.y + -requiredRotation.z * requiredRotation.x +
                    requiredRotation.y * requiredRotation.w,
                m20 =
                    requiredRotation.z * -requiredRotation.x + requiredRotation.w * -requiredRotation.y +
                    -requiredRotation.x * -requiredRotation.z,
                m21 =
                    requiredRotation.z * requiredRotation.w + requiredRotation.w * -requiredRotation.z +
                    -requiredRotation.x * requiredRotation.y,
                m22 =
                    requiredRotation.z * requiredRotation.z + requiredRotation.w * requiredRotation.w +
                    -requiredRotation.x * -requiredRotation.x,
                m23 =
                    requiredRotation.z * -requiredRotation.y + requiredRotation.w * requiredRotation.x +
                    -requiredRotation.x * requiredRotation.w,
                m30 =
                    -requiredRotation.y * -requiredRotation.x + requiredRotation.x * -requiredRotation.y +
                    requiredRotation.w * -requiredRotation.z,
                m31 =
                    -requiredRotation.y * requiredRotation.w + requiredRotation.x * -requiredRotation.z +
                    requiredRotation.w * requiredRotation.y,
                m32 =
                    -requiredRotation.y * requiredRotation.z + requiredRotation.x * requiredRotation.w +
                    requiredRotation.w * -requiredRotation.x,
                m33 =
                    -requiredRotation.y * -requiredRotation.y + requiredRotation.x * requiredRotation.x +
                    requiredRotation.w * requiredRotation.w,
            };

            Quaternion neededAngularVelocity = ComputeOutput(error, delta, deltaTime);

            neededAngularVelocity = MultiplyAsVector(orthogonalizeMatrix, neededAngularVelocity);

            Quaternion doubleNegative = neededAngularVelocity.Multiply(-2.0f);
            Quaternion result         = doubleNegative * Quaternion.Inverse(requiredRotation);

            return(new Vector3(result.x, result.y, result.z));
        }