/*---------------------------------------------------------------------------*
  Project:  NintendoWare
  File:     math_Matrix34.ipp
  
  Copyright (C)2009-2011 Nintendo/HAL Laboratory, Inc.  All rights reserved.
  
  These coded instructions, statements, and computer programs contain proprietary
  information of Nintendo and/or its licensed developers and are protected by
  national and international copyright laws. 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.
  
  The content herein is highly confidential and should be handled accordingly.
  
  $Revision: 31311 $
 *---------------------------------------------------------------------------*/
#include <cmath>
#include <nw/math/math_Vector3.h>

namespace nw { 
namespace math {
using namespace nn::math;

/*!--------------------------------------------------------------------------*
  @brief        カメラ行列を設定します。

  @param[out]   pOut     計算結果を受け取るバッファへのポインタ。
  @param[in]    pCamPos  カメラの位置を指定するベクトルへのポインタ。
  @param[in]    twist    カメラの視線方向に対する1 円周を 256.0 とする単位での回転角度。
  @param[in]    pTarget  カメラの注視点を指定するベクトルへのポインタ。

  @return       pOut を返します。
 *---------------------------------------------------------------------------*/
NW_MATH_INLINE MTX34*
MTX34LookAtFIdx(
    nw::math::MTX34* pOut, const nw::math::VEC3* pCamPos, f32 twist, const nw::math::VEC3* pTarget)
{
    using namespace nw::math;

    NW_NULL_ASSERT(pOut);
    NW_NULL_ASSERT(pCamPos);
    NW_NULL_ASSERT(pTarget);

    f32 (*const m)[4] = pOut->m;

    // カメラ座標系のＺ方向
    VEC3 lookReverse(pCamPos->x - pTarget->x, pCamPos->y - pTarget->y, pCamPos->z - pTarget->z);

    if ((lookReverse.x == 0.0f) && (lookReverse.z == 0.0f))
    {
        // カメラとターゲットのXZ座標が同じ場合ツイストは定義されない
        m[0][0] = 1.0f;
        m[0][1] = 0.0f;
        m[0][2] = 0.0f;
        m[0][3] = -pCamPos->x;

        m[1][0] = 0.0f;
        m[1][1] = 0.0f;

        m[2][0] = 0.0f;
        m[2][2] = 0.0f;

        if (lookReverse.y <= 0.0f)
        {
            // 真上を見るとき
            m[1][2] = 1.0f;
            m[1][3] = -pCamPos->z;

            m[2][1] = -1.0f;
            m[2][3] = pCamPos->y;
        }
        else
        {
            // 真下を見るとき
            m[1][2] = -1.0f;
            m[1][3] = pCamPos->z;

            m[2][1] = 1.0f;
            m[2][3] = -pCamPos->y;
        }
    }
    else
    {
        // カメラ座標系のＸ方向
        VEC3 r(lookReverse.z, 0.0f, -lookReverse.x);

        VEC3Normalize(&lookReverse, &lookReverse);
        VEC3Normalize(&r, &r);

        // カメラ座標系のＹ方向
        VEC3 u;
        VEC3Cross(&u, &lookReverse, &r);

        f32 st, ct;
        SinCosFIdx(&st, &ct, twist);
        VEC3 right, up;

        // r軸, u軸をru平面上でcameraTwistだけ半時計回りに回転させてrightを求める
        // r.y == 0であることに注意
        right.x = st * u.x + ct * r.x;
        right.y = st * u.y;
        right.z = st * u.z + ct * r.z;

        up.x    = ct * u.x - st * r.x;
        up.y    = ct * u.y;
        up.z    = ct * u.z - st * r.z;

        // right
        m[0][0] = right.x;
        m[0][1] = right.y;
        m[0][2] = right.z;
        m[0][3] = -VEC3Dot(pCamPos, &right);

        // up
        m[1][0] = up.x;
        m[1][1] = up.y;
        m[1][2] = up.z;
        m[1][3] = -VEC3Dot(pCamPos, &up);

        // look
        m[2][0] = lookReverse.x;
        m[2][1] = lookReverse.y;
        m[2][2] = lookReverse.z;
        m[2][3] = -VEC3Dot(pCamPos, &lookReverse);
    }

    return pOut;
}

/*!--------------------------------------------------------------------------*
  @brief        カメラ行列を設定します。

  @param[out]   pOut        計算結果を受け取るバッファへのポインタ。
  @param[in]    pCamPos     カメラの位置を指定するベクトルへのポインタ。
  @param[in]    pCamRotate  1 円周を 256.0 とする単位でのカメラの回転量を指定するベクトルへのポインタ。

  @return       pOut を返します。
 *---------------------------------------------------------------------------*/
NW_MATH_INLINE MTX34*
MTX34CameraRotateFIdx(
    nw::math::MTX34* pOut, const nw::math::VEC3* pCamPos, const nw::math::VEC3* pCamRotate)
{
    using namespace nw::math;

    NW_NULL_ASSERT(pOut);
    NW_NULL_ASSERT(pCamPos);
    NW_NULL_ASSERT(pCamRotate);

    f32 (*const m)[4] = pOut->m;

    f32 sx, sy, sz, cx, cy, cz;
    SinCosFIdx(&sx, &cx, pCamRotate->x);
    SinCosFIdx(&sy, &cy, pCamRotate->y);
    SinCosFIdx(&sz, &cz, pCamRotate->z);

    // Z軸, X軸, Y軸の順番で回転させている
    VEC3 right, up, back;

    right.x = sx * sy * sz + cy * cz;
    right.y = cx * sz;
    right.z = sx * cy * sz - sy * cz;

    up.x    = sx * sy * cz - cy * sz;
    up.y    = cx * cz;
    up.z    = sx * cy * cz + sy * sz;

    back.x  = cx * sy;
    back.y  = - sx;
    back.z  = cx * cy;

    m[0][0] = right.x;
    m[0][1] = right.y;
    m[0][2] = right.z;
    m[0][3] = -VEC3Dot(pCamPos, &right);

    m[1][0] = up.x;
    m[1][1] = up.y;
    m[1][2] = up.z;
    m[1][3] = -VEC3Dot(pCamPos, &up);

    m[2][0] = back.x;
    m[2][1] = back.y;
    m[2][2] = back.z;
    m[2][3] = -VEC3Dot(pCamPos, &back);

    return pOut;
}

/*!--------------------------------------------------------------------------*
  @brief        カメラ行列を設定します。

  @param[out]   pOut        計算結果を受け取るバッファへのポインタ。
  @param[in]    pCamPos     カメラの位置を指定するベクトルへのポインタ。
  @param[in]    pCamRotate  ラジアン単位でのカメラの回転量を指定するベクトルへのポインタ。

  @return       pOut を返します。
 *---------------------------------------------------------------------------*/
NW_MATH_INLINE MTX34*
MTX34CameraRotateRad(MTX34* pOut, const VEC3* pCamPos, const VEC3* pCamRotate)
{
    VEC3 rotateFIdx;
    rotateFIdx.x = NW_MATH_RAD_TO_FIDX(pCamRotate->x);
    rotateFIdx.y = NW_MATH_RAD_TO_FIDX(pCamRotate->y);
    rotateFIdx.z = NW_MATH_RAD_TO_FIDX(pCamRotate->z);

    return MTX34CameraRotateFIdx(pOut, pCamPos, &rotateFIdx);
}

/*!--------------------------------------------------------------------------*
  @brief        カメラ行列を設定します。

  @param[out]   pOut     計算結果を受け取るバッファへのポインタ。
  @param[in]    pCamPos  カメラの位置を指定するベクトルへのポインタ。
  @param[in]    twist    カメラの視線方向に対するラジアン単位での回転角度。
  @param[in]    pTarget  カメラの注視点を指定するベクトルへのポインタ。

  @return       pOut を返します。
 *---------------------------------------------------------------------------*/
NW_MATH_INLINE MTX34*
MTX34LookAtRad(MTX34* pOut, const VEC3* pCamPos, f32 twist, const VEC3* pTarget)
{
    f32 twistFIdx = NW_MATH_RAD_TO_FIDX(twist);
    return MTX34LookAtFIdx(pOut, pCamPos, twistFIdx, pTarget);
}

/*!--------------------------------------------------------------------------*
  @brief        行列からスケール値を取得します。
                右からかけたスケールを取り出します。

  @param[out]   pOut     計算結果を受け取るバッファへのポインタ。
  @param[in]    pM       スケールを取り出す行列へのポインタ。

  @return       pOut を返します。
 *---------------------------------------------------------------------------*/
NW_MATH_INLINE VEC3*
MTX34DecomposeToColumnScale(VEC3* pOut, const MTX34* pM)
{
    pOut->x = FSqrt((pM->f._00 * pM->f._00) + (pM->f._10 * pM->f._10) + (pM->f._20 * pM->f._20));
    pOut->y = FSqrt((pM->f._01 * pM->f._01) + (pM->f._11 * pM->f._11) + (pM->f._21 * pM->f._21));
    pOut->z = FSqrt((pM->f._02 * pM->f._02) + (pM->f._12 * pM->f._12) + (pM->f._22 * pM->f._22));

    return pOut;
}

/*!--------------------------------------------------------------------------*
  @brief        行列からスケール値を取得します。
                左からかけたスケールを取り出します。

  @param[out]   pOut     計算結果を受け取るバッファへのポインタ。
  @param[in]    pM       スケールを取り出す行列へのポインタ。

  @return       pOut を返します。
 *---------------------------------------------------------------------------*/
NW_MATH_INLINE VEC3*
MTX34DecomposeToRowScale(VEC3* pOut, const MTX34* pM)
{
    pOut->x = FSqrt((pM->f._00 * pM->f._00) + (pM->f._01 * pM->f._01) + (pM->f._02 * pM->f._02));
    pOut->y = FSqrt((pM->f._10 * pM->f._10) + (pM->f._11 * pM->f._11) + (pM->f._12 * pM->f._12));
    pOut->z = FSqrt((pM->f._20 * pM->f._20) + (pM->f._21 * pM->f._21) + (pM->f._22 * pM->f._22));

    return pOut;
}

}  // namespace math
}  // namespace nw