SinCos

Vector2<Vector> VFunction Vector::SinCos(const Vector& angles)
{
    Vector2<Vector> results;
    // Force the value within the bounds of pi
    Vector x = Vector::ModAngles(angles);

    // Map in [-pi/2,pi/2] with sin(y) = sin(x), cos(y) = sign*cos(x).
    Vector sign = _mm_and_ps(x, Vector::Constant::NegativeZero);
    __m128 c = _mm_or_ps(Vector::Constant::Pi, sign);  // pi when x >= 0, -pi when x < 0
    __m128 absx = _mm_andnot_ps(sign, x);  // |x|
    __m128 rflx = _mm_sub_ps(c, x);
    __m128 comp = _mm_cmple_ps(absx, Vector::Constant::HalfPi);
    __m128 select0 = _mm_and_ps(comp, x);
    __m128 select1 = _mm_andnot_ps(comp, rflx);
    x = _mm_or_ps(select0, select1);
    select0 = _mm_and_ps(comp, Vector::Constant::One);
    select1 = _mm_andnot_ps(comp, Vector::Constant::NegativeOne);
    sign = _mm_or_ps(select0, select1);

    __m128 x2 = _mm_mul_ps(x, x);

    // Compute polynomial approximation of sine
    const Vector SC1 = Vector::Constant::SinCoefficients1;
    Vector vConstants = Vector::Permute<0, 0, 0, 0>(SC1);
    __m128 Result = _mm_mul_ps(vConstants, x2);

    const Vector SC0 = Vector::Constant::SinCoefficients0;
    vConstants = Vector::Permute<3, 3, 3, 3>(SC0);
    Result = _mm_add_ps(Result, vConstants);
    Result = _mm_mul_ps(Result, x2);

    vConstants = Vector::Permute<2, 2, 2, 2>(SC0);
    Result = _mm_add_ps(Result, vConstants);
    Result = _mm_mul_ps(Result, x2);

    vConstants = Vector::Permute<1, 1, 1, 1>(SC0);
    Result = _mm_add_ps(Result, vConstants);
    Result = _mm_mul_ps(Result, x2);

    vConstants = Vector::Permute<0, 0, 0, 0>(SC0);
    Result = _mm_add_ps(Result, vConstants);
    Result = _mm_mul_ps(Result, x2);
    Result = _mm_add_ps(Result, Vector::Constant::One);
    Result = _mm_mul_ps(Result, x);
    //*pSin = Result;
    results.x = Result;

    // Compute polynomial approximation of cosine
    const Vector CC1 = Vector::Constant::CosCoefficients1;
    vConstants = Vector::Permute<0, 0, 0, 0>(CC1);
    Result = _mm_mul_ps(vConstants, x2);

    const Vector CC0 = Vector::Constant::CosCoefficients0;
    vConstants = Vector::Permute<3, 3, 3, 3>(CC0);
    Result = _mm_add_ps(Result, vConstants);
    Result = _mm_mul_ps(Result, x2);

    vConstants = Vector::Permute<2, 2, 2, 2>(CC0);
    Result = _mm_add_ps(Result, vConstants);
    Result = _mm_mul_ps(Result, x2);

    vConstants = Vector::Permute<1, 1, 1, 1>(CC0);
    Result = _mm_add_ps(Result, vConstants);
    Result = _mm_mul_ps(Result, x2);

    vConstants = Vector::Permute<0, 0, 0, 0>(CC0);
    Result = _mm_add_ps(Result, vConstants);
    Result = _mm_mul_ps(Result, x2);
    Result = _mm_add_ps(Result, Vector::Constant::One);
    Result = _mm_mul_ps(Result, sign);
    //*pCos = Result;
    results.y = Result;

    return results;
}