/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
 * 
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * 
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 
 * 3. This notice may not be removed or altered from any source distribution.
 */

#include "dgStdafx.h"
#include "dgGoogol.h"
#include "dgSmallDeterminant.h"

#define Absolute(a)  ((a) >= 0.0 ? (a) : -(a))

dgFloat64 Determinant2x2(const dgFloat64 matrix[2][2], dgFloat64* const error)
{
  dgFloat64 a00xa11 = matrix[0][0] * matrix[1][1];
  dgFloat64 a01xa10 = matrix[0][1] * matrix[1][0];
  *error = Absolute(a00xa11) + Absolute(a01xa10);
  return a00xa11 - a01xa10;
}

dgGoogol Determinant2x2(const dgGoogol matrix[2][2])
{
  dgGoogol a00xa11(matrix[0][0] * matrix[1][1]);
  dgGoogol a01xa10(matrix[0][1] * matrix[1][0]);
  return a00xa11 - a01xa10;
}

dgFloat64 Determinant3x3(const dgFloat64 matrix[3][3], dgFloat64* const error)
{
  dgFloat64 sign = dgFloat64(-1.0f);
  dgFloat64 det = dgFloat64(0.0f);
  dgFloat64 accError = dgFloat64(0.0f);
  for (dgInt32 i = 0; i < 3; i++)
  {
    dgFloat64 cofactor[2][2];
    for (dgInt32 j = 0; j < 2; j++)
    {
      dgInt32 k0 = 0;
      for (dgInt32 k = 0; k < 3; k++)
      {
        if (k != i)
        {
          cofactor[j][k0] = matrix[j][k];
          k0++;
        }
      }
    }

    dgFloat64 parcialError;
    dgFloat64 minorDet = Determinant2x2(cofactor, &parcialError);
    accError += parcialError * Absolute (matrix[2][i]);
    det += sign * minorDet * matrix[2][i];
    sign *= dgFloat64(-1.0f);
  }

  *error = accError;
  return det;
}

dgGoogol Determinant3x3(const dgGoogol matrix[3][3])
{
  dgGoogol negOne(dgFloat64(-1.0f));
  dgGoogol sign(dgFloat64(-1.0f));
  dgGoogol det = dgFloat64(0.0f);
  for (dgInt32 i = 0; i < 3; i++)
  {
    dgGoogol cofactor[2][2];

    for (dgInt32 j = 0; j < 2; j++)
    {
      dgInt32 k0 = 0;
      for (dgInt32 k = 0; k < 3; k++)
      {
        if (k != i)
        {
          cofactor[j][k0] = matrix[j][k];
          k0++;
        }
      }
    }

    dgGoogol minorDet(Determinant2x2(cofactor));
    det = det + sign * minorDet * matrix[2][i];
    sign = sign * negOne;
  }
  return det;
}

dgFloat64 Determinant4x4(const dgFloat64 matrix[4][4], dgFloat64* const error)
{
  dgFloat64 sign = dgFloat64(1.0f);
  dgFloat64 det = dgFloat64(0.0f);
  dgFloat64 accError = dgFloat64(0.0f);
  for (dgInt32 i = 0; i < 4; i++)
  {
    dgFloat64 cofactor[3][3];
    for (dgInt32 j = 0; j < 3; j++)
    {
      dgInt32 k0 = 0;
      for (dgInt32 k = 0; k < 4; k++)
      {
        if (k != i)
        {
          cofactor[j][k0] = matrix[j][k];
          k0++;
        }
      }
    }

    dgFloat64 parcialError;
    dgFloat64 minorDet = Determinant3x3(cofactor, &parcialError);
    accError += parcialError * Absolute (matrix[3][i]);
    det += sign * minorDet * matrix[3][i];
    sign *= dgFloat64(-1.0f);
  }

  *error = accError;
  return det;
}

dgGoogol Determinant4x4(const dgGoogol matrix[4][4])
{
  dgGoogol sign = dgFloat64(1.0f);
  dgGoogol det = dgFloat64(0.0f);
  dgGoogol negOne(dgFloat64(-1.0f));
  dgGoogol accError = dgFloat64(0.0f);
  for (dgInt32 i = 0; i < 4; i++)
  {
    dgGoogol cofactor[3][3];
    for (dgInt32 j = 0; j < 3; j++)
    {
      dgInt32 k0 = 0;
      for (dgInt32 k = 0; k < 4; k++)
      {
        if (k != i)
        {
          cofactor[j][k0] = matrix[j][k];
          k0++;
        }
      }
    }

    dgGoogol minorDet = Determinant3x3(cofactor);
    det = det + sign * minorDet * matrix[3][i];
    sign = sign * negOne;
  }
  return det;
}

