libraries/math/math_Equation.cpp

/*---------------------------------------------------------------------------*
  Project:  Horizon
  File:     math_Equation.cpp

  Copyright (C)2009 Nintendo Co., Ltd.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Rev: 12449 $
 *---------------------------------------------------------------------------*/

#include <nn/math.h>

#include <nn/math/math_Equation.h>
#include <nn/math/math_Arithmetic.h>
#include <nn/math/math_Triangular.h>
#include <nn/types.h>

namespace nn { namespace math {


namespace
{
    // 等しいと判定する最大誤差
    const f32 EPS = 2e-4f;


    /*!--------------------------------------------------------------------------*
      Name:        spow<int>

      @brief       対象の数の自然数乗を求めます。
                   最適化によって静的なコードが生成される事を期待します。

      @param[in]   idx     指数です。定数で指定しなければなりません。
      @param[in]   x       底です。

      @return      x の idx 乗を返します。
     *---------------------------------------------------------------------------*/
    template <int idx>
    NN_MATH_INLINE f32
    spow(f32 x)
    {
        return spow<idx-1>(x) * x;
    }

    template <>
    NN_MATH_INLINE f32
    spow<1>(f32 x)
    {
        return x;
    }

    /*!--------------------------------------------------------------------------*
      Name:        SolveEquation2

      @brief       二次方程式 x^2 + bx + c = 0 の実数解を求めます。
                   SolveEquation4 用です。

      @param[out]  root    実数解を格納する配列へのポインタ。
                   二次方程式には最大で2つの実数解が存在しますので、
                   配列の大きさは2以上でなければなりません。
      @param[in]   b,c     対象の二次方程式の係数。

      @return      実数解の個数。
                   root が指す配列の先頭から順に個数分の解が格納されます。
     *---------------------------------------------------------------------------*/
    int
    SolveEquation2(f32* root, /*f32 a==1,*/ f32 b, f32 c)
    {
        if( b == 0 )
        {
            if( c < -EPS )      // c < 0
            {
                f32 r_c = FSqrt(-c);
                root[0] =   r_c;
                root[1] = - r_c;
                return 2;
            }
            else if( c <= EPS ) // c == 0
            {
                root[0] = 0;
                return 1;
            }
            else                // c > 0
            {
                return 0;
            }
        }
        else
        {
            f32 A = b / 2;
            f32 B = c / spow<2>(A);
            f32 D = 1 - B;

            if( D > 0 )
            {
                f32 C = -1 - FSqrt(D);

                root[0] = A * C;
                root[1] = A * B / C;

                return 2;
            }
            else if( FAbs(D) < EPS )
            {
                root[0] = -A;

                return 1;
            }
            else
            {
                return 0;
            }
        }
    }


    /*!--------------------------------------------------------------------------*
      Name:        SolveEquation3

      @brief       三次方程式 x^3 + bx^2 + cx + d = 0 の実数解を1つ求めます。
                   SolveEquation4 用です。

      @param[in]   b,c,d   対象の三次方程式の係数。

      @return      実数解のうちのどれか1つの値。
     *---------------------------------------------------------------------------*/
    f32
    SolveEquation3(/*f32* root, f32 a==1,*/ f32 b, f32 c, f32 d)
    {
        f32 q   = (spow<2>(b) - 3 * c) / 9;
        f32 r   = (2 * spow<3>(b) - 9 * b * c + 27 * d) / 54;
        f32 D   = spow<3>(q) - spow<2>(r);
        f32 b_3 = b / 3;

        if( D > EPS )
        {
            f32 theta   = AcosRad( r / FSqrt(spow<3>(q)) );
            f32 theta_3 = theta/3;
            f32 r_q2    = -2 * FSqrt(q);

            return r_q2 * CosRad(theta_3) - b_3;
        }
        else if( D < - EPS )
        {
            f32 r3_Dr = FCbrt( FSqrt(- D) + FAbs(r) );
            f32 xp    = r3_Dr + q / r3_Dr;

            return - FCopySign(xp, r) - b_3;
        }
        else
        {
            f32 xp  = FSqrt(q);

            return FCopySign(xp, r) - b_3;
        }
    }
}


/*!--------------------------------------------------------------------------*
  Name:        SolveEquation2

  @brief       二次方程式 ax^2 + bx + c = 0 の実数解を求めます。

  @param[out]  root    実数解を格納する配列へのポインタ。
               二次方程式には最大で2つの実数解が存在しますので、
               配列の大きさは2以上でなければなりません。
  @param[in]   a,b,c   対象の二次方程式の係数。
               a が 0 であってはいけません。

  @return      実数解の個数。
               root が指す配列の先頭から順に個数分の解が格納されます。
 *---------------------------------------------------------------------------*/
int
SolveEquation2(f32* root, f32 a, f32 b, f32 c)
{
    NN_POINTER_TASSERT_(root);
    NN_TASSERT_( a != 0 );

    if( b != 0 )
    {
        // 一般的な解の公式
        //   x = {-b ± √(b^2 - 4ac)} / 2a
        // では -b ± √(略) の部分で -b の正負と ± の正負が異なる方の解を
        // 求める時に、近しい値の減算が発生し精度が大きく落ちます。
        // これを回避するため、解の一方を
        //   x = b{-1 - √(1 - 4ac/b^2)} / 2a
        // と b を括りだして求めます。
        // 他方の解には、上記式に解と係数の関係
        //   αβ = a / c
        // を適用して
        //   x = 2c / b{-1 - √(1 - 4ac/b^2)}
        // から求めます。

        f32 A = b / (2 * a);
        f32 B = c / (a * A * A);
        f32 D = 1 - B;

        if( D > EPS )
        {
            f32 C = -1 - FSqrt(D);

            root[0] = A * C;
            root[1] = A * B / C;

            return 2;
        }
        else if( D >= - EPS )
        {
            root[0] = -A;

            return 1;
        }
        else
        {
            return 0;
        }
    }
    else
    {
        // 上の方法では b = 0 の場合に 0 除算が発生するため場合分けします。

        f32 c_a = - c / a;

        if( c_a > EPS )
        {
            f32 r_c_a = FSqrt(c_a);
            root[0] = + r_c_a;
            root[1] = - r_c_a;
            return 2;
        }
        else if( c_a >= - EPS )
        {
            root[0] = 0;
            return 1;
        }
        else
        {
            return 0;
        }
    }
}


/*!--------------------------------------------------------------------------*
  Name:        SolveEquation3

  @brief       三次方程式 ax^3 + bx^2 + cx + d = 0 の実数解を求めます。

  @param[out]  root        実数解を格納する配列へのポインタ。
               三次方程式には最大で3つの実数解が存在しますので、
               配列の大きさは3以上でなければなりません。
  @param[in]   a,b,c,d     対象の三次方程式の係数。
               a が 0 であってはいけません。

  @return      実数解の個数。
               root が指す配列の先頭から順に個数分の解が格納されます。
 *---------------------------------------------------------------------------*/
int
SolveEquation3(f32* root, f32 a, f32 b, f32 c, f32 d)
{
    NN_POINTER_TASSERT_(root);
    NN_TASSERT_( a != 0 );

    //---- 両辺を最高次の係数で割って最高次の係数を 1 にします
    b /= a;
    c /= a;
    d /= a;

    //---- 判別式 D の値を求めます
    f32 q = (spow<2>(b) - 3 * c) / 9;
    f32 r = (2 * spow<3>(b) - 9 * b * c + 27 * d) / 54;
    f32 D = spow<3>(q) - spow<2>(r);

    f32 b_3 = b / 3;

    if( D > EPS )
    {
        f32 theta   = AcosRad( r / FSqrt(spow<3>(q)) );
        f32 theta_3 = theta/3;
        f32 r_q2    = -2 * FSqrt(q);

        root[0] = r_q2 * CosRad(theta_3 + 0 * F_PI / 3) - b_3;
        root[1] = r_q2 * CosRad(theta_3 + 2 * F_PI / 3) - b_3;
        root[2] = r_q2 * CosRad(theta_3 + 4 * F_PI / 3) - b_3;

        return 3;
    }
    else if( D < - EPS )
    {
        f32 r3_Dr = FCbrt( FSqrt(- D) + FAbs(r) );
        f32 xp    = r3_Dr + q / r3_Dr;

        root[0] = - FCopySign(xp, r) - b_3;

        return 1;
    }
    else
    {
        f32 xp  = FSqrt(q);
        f32 sxp = FCopySign(xp, r);

        root[0] =  1 * sxp - b_3;
        root[1] = -2 * sxp - b_3;

        return 2;
    }
}


/*!--------------------------------------------------------------------------*
  Name:        SolveEquation4

  @brief       四次方程式 ax^4 + bx^3 + cx^2 + dx + e = 0 の実数解を
               求めます。

  @param[out]  root        実数解を格納する配列へのポインタ。
               四次方程式には最大で4つの実数解が存在しますので、
               配列の大きさは4以上でなければなりません。
  @param[in]   a,b,c,d,e   対象の四次方程式の係数。
               a が 0 であってはいけません。

  @return      実数解の個数。
               root が指す配列の先頭から順に個数分の解が格納されます。
 *---------------------------------------------------------------------------*/
int
SolveEquation4(f32* root, f32 a, f32 b, f32 c, f32 d, f32 e)
{
    NN_POINTER_TASSERT_(root);
    NN_TASSERT_( a != 0 );

    f32 m, n, y;

    //---- 両辺を最高次の係数で割って最高次の係数を 1 にします
    b /= a;
    c /= a;
    d /= a;
    e /= a;

    //---- 平方完成
    {
        // t = x + b/4 と置くと x = t - b/4
        // これを x に代入して整理すると3次の項が消えて、
        //   t^4 + pt^2 + qt + r = 0
        // となる。ここで p, q, r はそれぞれ以下のとおり。

        f32 p = - 3.f * spow<2>(b) / 8 + c;
        f32 q = spow<3>(b) / 8 - b * c / 2 + d;
        f32 r = - 3.f * spow<4>(b) / 256 + spow<2>(b) * c / 16 - b * d / 4 + e;


        // t に関する式を以下の形に平方完成させる事を考える
        //   (t^2 + y)^2 = (mt + n)^2   ------- (1)

        if( q != 0 )
        {
            // まず t^2 に関して任意の値 y について
            //   (t^2 + y)^2 = 2yt^2 + y^2 - (pt^2 + qt + r)
            //               = (2y - p)t^2 + qt + (y^2 - r)   ------- (2)
            // と平方完成させる事ができる
            // さらに右辺も平方完成させるには右辺の二次式の判別式 D = 0 であればよいので
            //   q^2 - 4(2y - p)(y^2 - r) = 0
            // を満たす y を求めればよい。 y に関して整理すると
            //   y^3 - (p/2)y^2 - ry + (pr/2 - (q^2)/8) = 0
            // この三次式を解いて解の一つを y として採用する。

            y = SolveEquation3(-p / 2, -r, p * r / 2 - spow<2>(q) / 8);


            // (1)式の右辺を展開すると
            //   (mt + n)^2 = m^2t^2 + 2mnt + n^2
            // これを(2)式と比較して以下を得る

            n = FSqrt(spow<2>(y) - r);
            m = -q / (2 * n);
        }
        else
        {
            // q = 0 のとき上記三次方程式を解くと y = p/2, ±sqrt(r)
            // SolveEquation3 によって ±sqrt(r) が選ばれると n = 0 となり、
            // 0 除算が発生する。
            // さらに p = q = r = 0 の場合必ず n = 0 となる。
            // これらを回避する。

            y = p / 2;
            n = FSqrt(spow<2>(y) - r);
            m = 0;
        }
    }

    // 与式を(1)式の形に変形できたので、これを解く。
    // (1)式の両辺の平方根を取ると
    //   t^2 + y = ±(mt + n)
    // これは 2 つの二次式
    //   t^2 + mt + (y + n) = 0
    //   t^2 - mt + (y - n) = 0
    // を表すので、これらを解けば t の値が求まる。
    // さらに x = t - b/4 より x が求まる。

    int nRoot = 0;
    f32 b4    = b / 4;

    //---- 二次方程式１
    {
        f32 root01[2];

        int nRoot01 = SolveEquation2(root01, m, y + n);

        root[nRoot]   = root01[0] - b4;
        root[nRoot+1] = root01[1] - b4;
        nRoot += nRoot01;
    }

    //---- 二次方程式２
    {
        f32 root23[2];

        int nRoot23 = SolveEquation2(root23, -m, y - n);

        root[nRoot]   = root23[0] - b4;
        root[nRoot+1] = root23[1] - b4;
        nRoot += nRoot23;
    }

    return nRoot;
}


}}  // nw::math