double dgConvexHull3DFace::Evalue (const dgBigVector* const pointArray, const dgBigVector& point) const
{
const dgBigVector& p0 = pointArray[m_index[0]];
const dgBigVector& p1 = pointArray[m_index[1]];
const dgBigVector& p2 = pointArray[m_index[2]];
double matrix[3][3];
for (int32_t i = 0; i < 3; i ++) {
matrix[0][i] = p2[i] - p0[i];
matrix[1][i] = p1[i] - p0[i];
matrix[2][i] = point[i] - p0[i];
}
double error;
double det = Determinant3x3 (matrix, &error);
double precision = double (1.0f) / double (1<<24);
double errbound = error * precision;
if (fabs(det) > errbound) {
return det;
}
dgGoogol exactMatrix[3][3];
for (int32_t i = 0; i < 3; i ++) {
exactMatrix[0][i] = dgGoogol(p2[i]) - dgGoogol(p0[i]);
exactMatrix[1][i] = dgGoogol(p1[i]) - dgGoogol(p0[i]);
exactMatrix[2][i] = dgGoogol(point[i]) - dgGoogol(p0[i]);
}
dgGoogol exactDet (Determinant3x3(exactMatrix));
det = exactDet.GetAproximateValue();
return det;
}
FLOAT32 Matrix4x4::Determinant() const
{
// COMMENT : http://www.acm.org/pubs/tog/GraphicsGems/gems/MatrixInvert.c
return _11 * Determinant3x3(_22, _32, _42, _23, _33, _43, _24, _34, _44) -
_12 * Determinant3x3(_21, _31, _41, _23, _33, _43, _24, _34, _44) +
_13 * Determinant3x3(_21, _31, _41, _22, _32, _42, _24, _34, _44) -
_14 * Determinant3x3(_21, _31, _41, _22, _32, _42, _23, _33, _43);
}
hacd::HaF64 Determinant4x4 (const hacd::HaF64 matrix[4][4], hacd::HaF64* const error)
{
hacd::HaF64 sign = hacd::HaF64 (1.0f);
hacd::HaF64 det = hacd::HaF64 (0.0f);
hacd::HaF64 accError = hacd::HaF64 (0.0f);
for (hacd::HaI32 i = 0; i < 4; i ++) {
hacd::HaF64 cofactor[3][3];
for (hacd::HaI32 j = 0; j < 3; j ++) {
hacd::HaI32 k0 = 0;
for (hacd::HaI32 k = 0; k < 4; k ++) {
if (k != i) {
cofactor[j][k0] = matrix[j][k];
k0 ++;
}
}
}
hacd::HaF64 parcialError;
hacd::HaF64 minorDet = Determinant3x3 (cofactor, &parcialError);
accError += parcialError * Absolute (matrix[3][i]);
det += sign * minorDet * matrix[3][i];
sign *= hacd::HaF64 (-1.0f);
}
*error = accError;
return det;
}
Matrix4x4<Real> Invert (const Matrix4x4<Real> &m)
{
Real det = Determinant3x3 (m);
assert (std::fabs (det) > 0.000001);
Real oneOverDet = 1.0 / det;
Matrix4x4<Real> res;
res._m11 = ((m._m22 * m._m33) - (m._m23 * m._m32)) * oneOverDet;
res._m12 = ((m._m13 * m._m32) - (m._m12 * m._m33)) * oneOverDet;
res._m13 = ((m._m12 * m._m23) - (m._m13 * m._m22)) * oneOverDet;
res._m21 = ((m._m23 * m._m31) - (m._m21 * m._m33)) * oneOverDet;
res._m22 = ((m._m11 * m._m33) - (m._m13 * m._m31)) * oneOverDet;
res._m23 = ((m._m13 * m._m21) - (m._m11 * m._m23)) * oneOverDet;
res._m31 = ((m._m21 * m._m32) - (m._m22 * m._m31)) * oneOverDet;
res._m32 = ((m._m12 * m._m31) - (m._m11 * m._m32)) * oneOverDet;
res._m33 = ((m._m11 * m._m22) - (m._m12 * m._m21)) * oneOverDet;
res._tx = -((m._tx * res._m11) + (m._ty * res._m21) + (m._tz * res._m31));
res._ty = -((m._tx * res._m12) + (m._ty * res._m22) + (m._tz * res._m32));
res._tz = -((m._tx * res._m13) + (m._ty * res._m23) + (m._tz * res._m33));
return res;
}
//dgFloat64 dgDelaunayTetrahedralization::GetTetraVolume (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& p2, const dgBigVector& p3) const
dgFloat64 dgDelaunayTetrahedralization::GetTetraVolume(
const dgConvexHull4dTetraherum* const tetra) const
{
// dgBigVector p1p0 (p1.Sub4(p0));
// dgBigVector p2p0 (p2.Sub4(p0));
// dgBigVector p3p0 (p3.Sub4(p0));
// dgBigVector normal (p1p0.CrossProduct4 (p2p0, p3p0));
// dgFloat64 det = normal.m_w;
const dgHullVector* const points = &m_points[0];
const dgBigVector &p0 = points[tetra->m_faces[0].m_index[0]];
const dgBigVector &p1 = points[tetra->m_faces[0].m_index[1]];
const dgBigVector &p2 = points[tetra->m_faces[0].m_index[2]];
const dgBigVector &p3 = points[tetra->m_faces[0].m_otherVertex];
dgFloat64 matrix[3][3];
for (dgInt32 i = 0; i < 3; i++)
{
matrix[0][i] = p2[i] - p0[i];
matrix[1][i] = p1[i] - p0[i];
matrix[2][i] = p3[i] - p0[i];
}
dgFloat64 error;
dgFloat64 det = Determinant3x3(matrix, &error);
dgFloat64 precision = dgFloat64(1.0f) / dgFloat64(1 << 24);
dgFloat64 errbound = error * precision;
if (fabs(det) > errbound)
{
return det;
}
dgGoogol exactMatrix[3][3];
for (dgInt32 i = 0; i < 3; i++)
{
exactMatrix[0][i] = dgGoogol(p2[i]) - dgGoogol(p0[i]);
exactMatrix[1][i] = dgGoogol(p1[i]) - dgGoogol(p0[i]);
exactMatrix[2][i] = dgGoogol(p3[i]) - dgGoogol(p0[i]);
}
dgGoogol exactDet(Determinant3x3(exactMatrix));
det = exactDet.GetAproximateValue();
return det;
}
static FLOAT32 MinorDeterminant(const Matrix4x4& rkMat, UINT32 uiRow, UINT32 uiCol)
{
// COMMENT : http://www.codeproject.com/csharp/Matrix.asp
FLOAT32 fM3x3[3][3];
for(UINT32 r = 0, m = 0; r < 4; ++r)
{
if(r == uiRow) {continue;}
for(UINT32 c = 0, n = 0; c < 4; ++c)
{
if(c == uiCol) {continue;}
fM3x3[m][n] = rkMat.m[r][c];
++n;
}
++m;
}
return Determinant3x3(fM3x3[0][0], fM3x3[0][1], fM3x3[0][2],
fM3x3[1][0], fM3x3[1][1], fM3x3[1][2],
fM3x3[2][0], fM3x3[2][1], fM3x3[2][2]);
}
dgGoogol Determinant4x4 (const dgGoogol matrix[4][4])
{
dgGoogol sign = hacd::HaF64 (1.0f);
dgGoogol det = hacd::HaF64 (0.0f);
dgGoogol negOne (hacd::HaF64 (-1.0f));
dgGoogol accError = hacd::HaF64 (0.0f);
for (hacd::HaI32 i = 0; i < 4; i ++) {
dgGoogol cofactor[3][3];
for (hacd::HaI32 j = 0; j < 3; j ++) {
hacd::HaI32 k0 = 0;
for (hacd::HaI32 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;
}
