using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

namespace Library
{
    public static partial class VectorMath
    {
        public static Vector3 Rotate(Vector3 v, Vector3 axis, float angle)
        {
            Vector3 result = new Vector3();

            float tr = t(angle);
            float cos = c(angle);
            float sin = s(angle);

            result.X = a1(angle, axis, tr, cos) * v.X + a2(angle, axis, tr, sin) * v.Y + a3(angle, axis, tr, sin) * v.Z;
            result.Y = b1(angle, axis, tr, sin) * v.X + b2(angle, axis, tr, cos) * v.Y + b3(angle, axis, tr, sin) * v.Z;
            result.Z = c1(angle, axis, tr, sin) * v.X + c2(angle, axis, tr, sin) * v.Y + c3(angle, axis, tr, cos) * v.Z; 

            return result;
        }

        private static float t(float angle)
        {
            return 1-(float)Math.Cos((double)angle);
        }

        private static float c(float angle)
        {
            return (float)Math.Cos((double)angle);
        }

        private static float s(float angle)
        {
            return (float)Math.Sin((double)angle);
        }

        private static float a1(float angle, Vector3 axis, float tr, float cos)
        {
            return (tr * axis.X * axis.X) + cos;
        }

        private static float a2(float angle, Vector3 axis, float tr, float sin)
        {
            return (tr * axis.X * axis.Y) - (sin * axis.Z);
        }

        private static float a3(float angle, Vector3 axis, float tr, float sin)
        {
            return (tr * axis.X * axis.Z) + (sin * axis.Y);
        }

        private static float b1(float angle, Vector3 axis, float tr, float sin)
        {
            return (tr * axis.X * axis.Y) + (sin * axis.Z);
        }

        private static float b2(float angle, Vector3 axis, float tr, float cos)
        {
            return (tr * axis.Y * axis.Y) + cos;
        }

        private static float b3(float angle, Vector3 axis, float tr, float sin)
        {
            return (tr * axis.Y * axis.Z) - (sin * axis.X);
        }

        private static float c1(float angle, Vector3 axis, float tr, float sin)
        {
            return (tr * axis.X * axis.Z) - (sin * axis.Y);
        }

        private static float c2(float angle, Vector3 axis, float tr, float sin)
        {
            return (tr * axis.Y * axis.Z) + (sin * axis.X);
        }

        private static float c3(float angle, Vector3 axis, float tr, float cos)
        {
            return (tr * axis.Z * axis.Z) + cos;
        }
    }
}