RICPP_INTERN(RtFloat) RiCPPDeterminant(RtMatrix matrix)
{
// Assign to individual variable names to aid selecting correct elements
RtFloat a1 = matrix[0][0];
RtFloat b1 = matrix[0][1];
RtFloat c1 = matrix[0][2];
RtFloat d1 = matrix[0][3];
RtFloat a2 = matrix[1][0];
RtFloat b2 = matrix[1][1];
RtFloat c2 = matrix[1][2];
RtFloat d2 = matrix[1][3];
RtFloat a3 = matrix[2][0];
RtFloat b3 = matrix[2][1];
RtFloat c3 = matrix[2][2];
RtFloat d3 = matrix[2][3];
RtFloat a4 = matrix[3][0];
RtFloat b4 = matrix[3][1];
RtFloat c4 = matrix[3][2];
RtFloat d4 = matrix[3][3];
return
a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
}
/*!
* Find determinant of a matrix.
*
* \ingroup matrix
*/
Lib3dsFloat
lib3ds_matrix_det(Lib3dsMatrix m)
{
Lib3dsFloat a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
a1 = m[0][0];
b1 = m[1][0];
c1 = m[2][0];
d1 = m[3][0];
a2 = m[0][1];
b2 = m[1][1];
c2 = m[2][1];
d2 = m[3][1];
a3 = m[0][2];
b3 = m[1][2];
c3 = m[2][2];
d3 = m[3][2];
a4 = m[0][3];
b4 = m[1][3];
c4 = m[2][3];
d4 = m[3][3];
return(
a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4)-
b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4)+
c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4)-
d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4)
);
}
double det4x4 (Matrix mat)
{
double ans;
double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
a1 = mat[0][0];
b1 = mat[0][1];
c1 = mat[0][2];
d1 = mat[0][3];
a2 = mat[1][0];
b2 = mat[1][1];
c2 = mat[1][2];
d2 = mat[1][3];
a3 = mat[2][0];
b3 = mat[2][1];
c3 = mat[2][2];
d3 = mat[2][3];
a4 = mat[3][0];
b4 = mat[3][1];
c4 = mat[3][2];
d4 = mat[3][3];
ans = a1 * det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4) -
b1 * det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4) +
c1 * det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4) -
d1 * det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
return ans;
}
bool Triangle::intersect(const Ray &r, Hit &h, float tmin)
{
Vec3f r0 = r.getOrigin();
Vec3f rd = r.getDirection();
Vec3f E1 = a - b;
Vec3f E2 = a - c;
Vec3f S = a - r0;
//参数写错，rd写成r0了……
float de = det3x3(rd.x(), rd.y(), rd.z(), E1.x(), E1.y(), E1.z(), E2.x(), E2.y(), E2.z());
if (de == 0.0f)
return false;
float t = det3x3(S.x(), S.y(), S.z(), E1.x(), E1.y(), E1.z(), E2.x(), E2.y(), E2.z())/de;
float belta = det3x3(rd.x(), rd.y(), rd.z(), S.x(), S.y(), S.z(), E2.x(), E2.y(), E2.z()) / de;
float lamda = det3x3(rd.x(), rd.y(), rd.z(), E1.x(), E1.y(), E1.z(), S.x(), S.y(), S.z()) / de;
Vec3f normal;
Vec3f::Cross3(normal, b - a, c - a);
normal.Normalize();
h.set(t, material, normal, r);
if (t >= tmin && belta > 0.0f && lamda > 0.0f && belta + lamda < 1.0f)
return true;
else
return false;
}
/*
* double = det4x4( matrix )
*
* calculate the determinant of a 4x4 matrix.
*/
double det4x4( Matrix4 *m )
{
double ans;
double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
/* assign to individual variable names to aid selecting */
/* correct elements */
a1 = m->element[0][0]; b1 = m->element[0][1];
c1 = m->element[0][2]; d1 = m->element[0][3];
a2 = m->element[1][0]; b2 = m->element[1][1];
c2 = m->element[1][2]; d2 = m->element[1][3];
a3 = m->element[2][0]; b3 = m->element[2][1];
c3 = m->element[2][2]; d3 = m->element[2][3];
a4 = m->element[3][0]; b4 = m->element[3][1];
c4 = m->element[3][2]; d4 = m->element[3][3];
ans = a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
- b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
+ c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
- d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
return ans;
}
float det4x4(float a1, float a2, float a3, float a4,
float b1, float b2, float b3, float b4,
float c1, float c2, float c3, float c4,
float d1, float d2, float d3, float d4) {
return
a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
- b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
+ c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
- d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
}
float glmatrixf::determinant() const
{
float a1 = v[0], a2 = v[1], a3 = v[2], a4 = v[3],
b1 = v[4], b2 = v[5], b3 = v[6], b4 = v[7],
c1 = v[8], c2 = v[9], c3 = v[10], c4 = v[11],
d1 = v[12], d2 = v[13], d3 = v[14], d4 = v[15];
return a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4)
- b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4)
+ c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4)
- d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
}
float det4x4(Matrix4By4 m)
{ float a1,a2,a3,a4,b1,b2,b3,b4;
float c1,c2,c3,c4,d1,d2,d3,d4,ans;
a1=m[0][0];b1=m[0][1];c1=m[0][2];d1=m[0][3];
a2=m[1][0];b2=m[1][1];c2=m[1][2];d2=m[1][3];
a3=m[2][0];b3=m[2][1];c3=m[2][2];d3=m[2][3];
a4=m[3][0];b4=m[3][1];c4=m[3][2];d4=m[3][3];
ans= a1*det3x3(b2,b3,b4,c2,c3,c4,d2,d3,d4)
-b1*det3x3(a2,a3,a4,c2,c3,c4,d2,d3,d4)
+c1*det3x3(a2,a3,a4,b2,b3,b4,d2,d3,d4)
-d1*det3x3(a2,a3,a4,b2,b3,b4,c2,c3,c4);
return(ans);
}
/** We always perform the calculation in doubles, to avoid prematurely losing
precision along the way. This relies on the compiler automatically
promoting our SkMScalar values to double (if needed).
*/
double SkMatrix44::determinant() const {
return fMat[0][0] * det3x3(fMat[1][1], fMat[1][2], fMat[1][3],
fMat[2][1], fMat[2][2], fMat[2][3],
fMat[3][1], fMat[3][2], fMat[3][3]) -
fMat[1][0] * det3x3(fMat[0][1], fMat[0][2], fMat[0][3],
fMat[2][1], fMat[2][2], fMat[2][3],
fMat[3][1], fMat[3][2], fMat[3][3]) +
fMat[2][0] * det3x3(fMat[0][1], fMat[0][2], fMat[0][3],
fMat[1][1], fMat[1][2], fMat[1][3],
fMat[3][1], fMat[3][2], fMat[3][3]) -
fMat[3][0] * det3x3(fMat[0][1], fMat[0][2], fMat[0][3],
fMat[1][1], fMat[1][2], fMat[1][3],
fMat[2][1], fMat[2][2], fMat[2][3]);
}
std::vector<double> computeInterpolationWeights3d(const GenericPoint& thepoint,
const Container& pt_v)
{
if (pt_v.size() !=4)
{
std::cout << " Compute interpolation weights.. error.. wrong number of points: " << pt_v.size() << std::endl;
return std::vector<double>();
}
double c1[3], c2[3], c3[3];
c1[0] = pt_v[1][0] - pt_v[0][0];
c1[1] = pt_v[1][1] - pt_v[0][1];
c1[2] = pt_v[1][2] - pt_v[0][2];
c2[0] = pt_v[2][0] - pt_v[0][0];
c2[1] = pt_v[2][1] - pt_v[0][1];
c2[2] = pt_v[2][2] - pt_v[0][2];
c3[0] = pt_v[3][0] - pt_v[0][0];
c3[1] = pt_v[3][1] - pt_v[0][1];
c3[2] = pt_v[3][2] - pt_v[0][2];
double det(det3x3(c1, c2, c3));
if (det == 0.0)
{
throw gsse::numerical_calculation_error(":: interpolation :: 3D :: coefficients.. determinant to small.. ");
}
double rhs[3];
std::vector<double> weights(4);
rhs[0] = thepoint[0] - pt_v[0][0];
rhs[1] = thepoint[1] - pt_v[0][1];
rhs[2] = thepoint[2] - pt_v[0][2];
weights[1] = det3x3(rhs, c2, c3) / det;
weights[2] = det3x3(c1, rhs, c3) / det;
weights[3] = det3x3(c1, c2, rhs) / det;
weights[0] = 1.0 - weights[1] - weights[2] - weights[3];
return weights; // here .. copy constructor // think about it.. [RH]
}
std::vector<double> solve3x3LinearSystem(std::vector<double>& _A, std::vector<double>& _b, double _EPS) {
std::vector<double> A(9, 0);
std::vector<double> b(3, 0);
double detA = det3x3(_A);
if (fabs(detA) < _EPS)
return{};
for (int i = 0; i < 3; i++)
b[i] = _b[i];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 9; j++)
A[j] = _A[j];
for (int j = 0; j < 3; j++)
A[j * 3 + i] = b[j];
_b[i] = det3x3(A) / detA;
}
return _b;
}
//----------------------------------------------------------------------------------------------
float Matrix::Determinant() const
{
return _11 * det3x3(_22, _23, _24,
_32, _33, _34,
_42, _43, _44) -
_12 * det3x3(_21, _23, _24,
_31, _33, _34,
_41, _43, _44) -
_13 * det3x3(_21, _22, _24,
_31, _32, _34,
_41, _42, _44) +
_14 * det3x3(_21, _22, _23,
_31, _32, _33,
_41, _42, _43);
}
void MatrixTest::testMatrixOperations ()
{
// Test computing determinant
Matrix<3,3,int> matrix;
assignList(matrix) = 1, 2, 3,
4, 5, 6,
7, 8, 9;
validateEquals (0, det3x3(matrix));
assignList(matrix) = -1, 2, 3,
4, -5, 2,
-2, 3, 1;
validateEquals (1, det3x3(matrix)); // computed by octave
// Test streaming
Matrix<2,2,int> matrix2;
assignList(matrix2) = 1, 2, 3, 4;
std::ostringstream stream;
stream << matrix2;
validateEquals (stream.str(), std::string("1, 2; 3, 4"));
// Test matrix multiply scalar
matrix2 =matrix2*2;
validateEquals(matrix2(0,0),2);
validateEquals(matrix2(0,1),4);
validateEquals(matrix2(1,0),6);
validateEquals(matrix2(1,1),8);
// Test matrix add matrix
// Matrix<2,2,int> matrix3;
// assignList(matrix3) = 1, 2, 3, 4;
// matrix3=matrix3+matrix2;
// validateEquals(matrix3(0,0),3);
// validateEquals(matrix3(0,1),6);
// validateEquals(matrix3(1,0),9);
// validateEquals(matrix3(1,1),12);
// Test matrix square
Matrix<2,2,double> matrix4;
assignList(matrix4) = 4.0, 9.0, 16.0, 25.0;
matrix4=sqrt(matrix4);
validateEquals(matrix4(0,0),2.0);
validateEquals(matrix4(0,1),3.0);
validateEquals(matrix4(1,0),4.0);
validateEquals(matrix4(1,1),5.0);
}
void DenseMatImpl::myDet(RingElem& d) const
{
if (myNumRows()==2)
{ det2x2(d, ConstMatrixView(this)); return; }
if (myNumRows()==3)
{ det3x3(d, ConstMatrixView(this)); return; }
if (IsField(myR))
{ DetByGauss(d, ConstMatrixView(this)); return; }
if (IsIntegralDomain(myR))
{ d = DetByBareiss(ConstMatrixView(this)); return; }
CoCoA_ERROR(ERR::NYI, "det for non integral domain");
}
scalar_t Determinant(const matrix4x4 &m)
{
scalar_t a1 = m[0][0];
scalar_t a2 = m[1][0];
scalar_t a3 = m[2][0];
scalar_t a4 = m[3][0];
scalar_t b1 = m[0][1];
scalar_t b2 = m[1][1];
scalar_t b3 = m[2][1];
scalar_t b4 = m[3][1];
scalar_t c1 = m[0][2];
scalar_t c2 = m[1][2];
scalar_t c3 = m[2][2];
scalar_t c4 = m[3][2];
scalar_t d1 = m[0][3];
scalar_t d2 = m[1][3];
scalar_t d3 = m[2][3];
scalar_t d4 = m[3][3];
return a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
}
int inverse_transpose3x3(T* m, T* out)
{
T det = det3x3(m);
if( det == 0 ) return 1;
det = T(1) / det;
out[0] = (m[R22]*m[R33]-m[R23]*m[R32])*det;
out[3] = (m[R32]*m[R13]-m[R33]*m[R12])*det;
out[6] = (m[R12]*m[R23]-m[R13]*m[R22])*det;
out[1] = (m[R23]*m[R31]-m[R21]*m[R33])*det;
out[4] = (m[R11]*m[R33]-m[R13]*m[R31])*det;
out[7] = (m[R21]*m[R13]-m[R23]*m[R11])*det;
out[2] = (m[R21]*m[R32]-m[R22]*m[R31])*det;
out[5] = (m[R12]*m[R31]-m[R11]*m[R32])*det;
out[8] = (m[R11]*m[R22]-m[R12]*m[R21])*det;
return 0;
}
int MHexahedron::getVolumeSign()
{
double mat[3][3];
mat[0][0] = _v[1]->x() - _v[0]->x();
mat[0][1] = _v[3]->x() - _v[0]->x();
mat[0][2] = _v[4]->x() - _v[0]->x();
mat[1][0] = _v[1]->y() - _v[0]->y();
mat[1][1] = _v[3]->y() - _v[0]->y();
mat[1][2] = _v[4]->y() - _v[0]->y();
mat[2][0] = _v[1]->z() - _v[0]->z();
mat[2][1] = _v[3]->z() - _v[0]->z();
mat[2][2] = _v[4]->z() - _v[0]->z();
double d = det3x3(mat);
if(d < 0.) return -1;
else if(d > 0.) return 1;
else return 0;
}
// Inverse matrix of a 3x3 matrix a
static inline void inv_3x3(float a[3][3], float inv[3][3])
{
float det = det3x3(a[0][0], a[0][1], a[0][2], a[1][0], a[1][1],
a[1][2], a[2][0], a[2][1], a[2][2]);
inv[0][0] = (a[1][1]*a[2][2] - a[2][1]*a[1][2])/det;
inv[1][0] = - (a[1][0]*a[2][2] - a[2][0]*a[1][2])/det;
inv[2][0] = (a[1][0]*a[2][1] - a[2][0]*a[1][1])/det;
inv[0][1] = - (a[0][1]*a[2][2] - a[2][1]*a[0][2])/det;
inv[1][1] = (a[0][0]*a[2][2] - a[2][0]*a[0][2])/det;
inv[2][1] = - (a[0][0]*a[2][1] - a[2][0]*a[0][1])/det;
inv[0][2] = (a[0][1]*a[1][2] - a[1][1]*a[0][2])/det;
inv[1][2] = - (a[0][0]*a[1][2] - a[1][0]*a[0][2])/det;
inv[2][2] = (a[0][0]*a[1][1] - a[1][0]*a[0][1])/det;
}
static double
determinant(const struct weston_matrix *m)
{
double det = 0;
#if 1
/* develop on last row */
det -= m->d[3 + 0 * 4] * det3x3(&m->d[4], &m->d[8], &m->d[12]);
det += m->d[3 + 1 * 4] * det3x3(&m->d[0], &m->d[8], &m->d[12]);
det -= m->d[3 + 2 * 4] * det3x3(&m->d[0], &m->d[4], &m->d[12]);
det += m->d[3 + 3 * 4] * det3x3(&m->d[0], &m->d[4], &m->d[8]);
#else
/* develop on first row */
det += m->d[0 + 0 * 4] * det3x3(&m->d[5], &m->d[9], &m->d[13]);
det -= m->d[0 + 1 * 4] * det3x3(&m->d[1], &m->d[9], &m->d[13]);
det += m->d[0 + 2 * 4] * det3x3(&m->d[1], &m->d[5], &m->d[13]);
det -= m->d[0 + 3 * 4] * det3x3(&m->d[1], &m->d[5], &m->d[9]);
#endif
return det;
}
static int inv3x3tran(double mat[3][3], double inv[3][3], double *det)
{
double ud;
*det = det3x3(mat);
if(*det == 0.0) return (0);
ud = 1. / (*det);
inv[0][0] = ud * (mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]);
inv[0][1] = -ud * (mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]);
inv[0][2] = ud * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]);
inv[1][0] = -ud * (mat[0][1] * mat[2][2] - mat[0][2] * mat[2][1]);
inv[1][1] = ud * (mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0]);
inv[1][2] = -ud * (mat[0][0] * mat[2][1] - mat[0][1] * mat[2][0]);
inv[2][0] = ud * (mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1]);
inv[2][1] = -ud * (mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0]);
inv[2][2] = ud * (mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]);
return 1;
}
mat4 & invert(mat4& B, const mat4& A)
{
nv_scalar det,oodet;
B.a00 = det3x3(A.a11, A.a21, A.a31, A.a12, A.a22, A.a32, A.a13, A.a23, A.a33);
B.a10 = -det3x3(A.a10, A.a20, A.a30, A.a12, A.a22, A.a32, A.a13, A.a23, A.a33);
B.a20 = det3x3(A.a10, A.a20, A.a30, A.a11, A.a21, A.a31, A.a13, A.a23, A.a33);
B.a30 = -det3x3(A.a10, A.a20, A.a30, A.a11, A.a21, A.a31, A.a12, A.a22, A.a32);
B.a01 = -det3x3(A.a01, A.a21, A.a31, A.a02, A.a22, A.a32, A.a03, A.a23, A.a33);
B.a11 = det3x3(A.a00, A.a20, A.a30, A.a02, A.a22, A.a32, A.a03, A.a23, A.a33);
B.a21 = -det3x3(A.a00, A.a20, A.a30, A.a01, A.a21, A.a31, A.a03, A.a23, A.a33);
B.a31 = det3x3(A.a00, A.a20, A.a30, A.a01, A.a21, A.a31, A.a02, A.a22, A.a32);
B.a02 = det3x3(A.a01, A.a11, A.a31, A.a02, A.a12, A.a32, A.a03, A.a13, A.a33);
B.a12 = -det3x3(A.a00, A.a10, A.a30, A.a02, A.a12, A.a32, A.a03, A.a13, A.a33);
B.a22 = det3x3(A.a00, A.a10, A.a30, A.a01, A.a11, A.a31, A.a03, A.a13, A.a33);
B.a32 = -det3x3(A.a00, A.a10, A.a30, A.a01, A.a11, A.a31, A.a02, A.a12, A.a32);
B.a03 = -det3x3(A.a01, A.a11, A.a21, A.a02, A.a12, A.a22, A.a03, A.a13, A.a23);
B.a13 = det3x3(A.a00, A.a10, A.a20, A.a02, A.a12, A.a22, A.a03, A.a13, A.a23);
B.a23 = -det3x3(A.a00, A.a10, A.a20, A.a01, A.a11, A.a21, A.a03, A.a13, A.a23);
B.a33 = det3x3(A.a00, A.a10, A.a20, A.a01, A.a11, A.a21, A.a02, A.a12, A.a22);
det = (A.a00 * B.a00) + (A.a01 * B.a10) + (A.a02 * B.a20) + (A.a03 * B.a30);
oodet = nv_one / det;
B.a00 *= oodet;
B.a10 *= oodet;
B.a20 *= oodet;
B.a30 *= oodet;
B.a01 *= oodet;
B.a11 *= oodet;
B.a21 *= oodet;
B.a31 *= oodet;
B.a02 *= oodet;
B.a12 *= oodet;
B.a22 *= oodet;
B.a32 *= oodet;
B.a03 *= oodet;
B.a13 *= oodet;
B.a23 *= oodet;
B.a33 *= oodet;
return B;
}
void adjoint( Matrix4 *in, Matrix4 *out )
{
double a1, a2, a3, a4, b1, b2, b3, b4;
double c1, c2, c3, c4, d1, d2, d3, d4;
/* assign to individual variable names to aid */
/* selecting correct values */
a1 = in->element[0][0]; b1 = in->element[0][1];
c1 = in->element[0][2]; d1 = in->element[0][3];
a2 = in->element[1][0]; b2 = in->element[1][1];
c2 = in->element[1][2]; d2 = in->element[1][3];
a3 = in->element[2][0]; b3 = in->element[2][1];
c3 = in->element[2][2]; d3 = in->element[2][3];
a4 = in->element[3][0]; b4 = in->element[3][1];
c4 = in->element[3][2]; d4 = in->element[3][3];
/* row column labeling reversed since we transpose rows & columns */
out->element[0][0] = det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
out->element[1][0] = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
out->element[2][0] = det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
out->element[3][0] = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
out->element[0][1] = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
out->element[1][1] = det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
out->element[2][1] = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
out->element[3][1] = det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
out->element[0][2] = det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
out->element[1][2] = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
out->element[2][2] = det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
out->element[3][2] = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
out->element[0][3] = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
out->element[1][3] = det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
out->element[2][3] = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
out->element[3][3] = det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
}
void glmatrixf::adjoint(const glmatrixf &m)
{
float a1 = m.v[0], a2 = m.v[1], a3 = m.v[2], a4 = m.v[3],
b1 = m.v[4], b2 = m.v[5], b3 = m.v[6], b4 = m.v[7],
c1 = m.v[8], c2 = m.v[9], c3 = m.v[10], c4 = m.v[11],
d1 = m.v[12], d2 = m.v[13], d3 = m.v[14], d4 = m.v[15];
v[0] = det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
v[1] = -det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
v[2] = det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
v[3] = -det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
v[4] = -det3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
v[5] = det3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
v[6] = -det3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
v[7] = det3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
v[8] = det3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
v[9] = -det3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
v[10] = det3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
v[11] = -det3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
v[12] = -det3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
v[13] = det3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
v[14] = -det3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
v[15] = det3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
}
bool glmatrix::invert(const glmatrix &m, double mindet)
{
double a1 = m.a.x, a2 = m.a.y, a3 = m.a.z, a4 = m.a.w,
b1 = m.b.x, b2 = m.b.y, b3 = m.b.z, b4 = m.b.w,
c1 = m.c.x, c2 = m.c.y, c3 = m.c.z, c4 = m.c.w,
d1 = m.d.x, d2 = m.d.y, d3 = m.d.z, d4 = m.d.w,
det1 = det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4),
det2 = -det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4),
det3 = det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4),
det4 = -det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4),
det = a1*det1 + b1*det2 + c1*det3 + d1*det4;
if(fabs(det) < mindet) return false;
double invdet = 1/det;
a.x = det1 * invdet;
a.y = det2 * invdet;
a.z = det3 * invdet;
a.w = det4 * invdet;
b.x = -det3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4) * invdet;
b.y = det3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4) * invdet;
b.z = -det3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4) * invdet;
b.w = det3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4) * invdet;
c.x = det3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4) * invdet;
c.y = -det3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4) * invdet;
c.z = det3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4) * invdet;
c.w = -det3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4) * invdet;
d.x = -det3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3) * invdet;
d.y = det3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3) * invdet;
d.z = -det3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3) * invdet;
d.w = det3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3) * invdet;
return true;
}
void Triangles::IntersectConsMetric(const double * s,const Int4 nbsol,const int * typsols,
const Real8 hmin1,const Real8 hmax1,const Real8 coef,
const Real8 anisomax ,const Real8 CutOff,const int NbJacobi,
const int DoNormalisation,const int choice)
{ // the array of solution s is store
// sol0,sol1,...,soln on vertex 0
// sol0,sol1,...,soln on vertex 1
// etc.
const int dim = 2;
int sizeoftype[] = { 1, dim ,dim * (dim+1) / 2, dim * dim } ;
// computation of the nb of field
Int4 ntmp = 0;
if (typsols)
{
for (Int4 i=0;i<nbsol;i++)
ntmp += sizeoftype[typsols[i]];
}
else
ntmp = nbsol;
// n is the total number of fields
const Int4 n = ntmp;
Int4 i,k,iA,iB,iC,iv;
R2 O(0,0);
int RelativeMetric = CutOff>1e-30;
Real8 hmin = Max(hmin1,MinimalHmin());
Real8 hmax = Min(hmax1,MaximalHmax());
Real8 coef2 = 1/(coef*coef);
if(verbosity>1)
{
cout << " -- Construction of Metric: Nb of sol. " << n << " nbt = "
<< nbt << " nbv= " << nbv
<< " coef = " << coef << endl
<< " hmin = " << hmin << " hmax=" << hmax
<< " anisomax = " << anisomax << " Nb Jacobi " << NbJacobi ;
if (RelativeMetric)
cout << " RelativeErr with CutOff= " << CutOff << endl;
else
cout << " Absolute Err" <<endl;
}
double *ss=(double*)s, *ssiii = ss;
double sA,sB,sC;
Real8 *detT = new Real8[nbt];
Real8 *Mmass= new Real8[nbv];
Real8 *Mmassxx= new Real8[nbv];
Real8 *dxdx= new Real8[nbv];
Real8 *dxdy= new Real8[nbv];
Real8 *dydy= new Real8[nbv];
Real8 *workT= new Real8[nbt];
Real8 *workV= new Real8[nbv];
int *OnBoundary = new int[nbv];
for (iv=0;iv<nbv;iv++)
{
Mmass[iv]=0;
OnBoundary[iv]=0;
Mmassxx[iv]=0;
}
for (i=0;i<nbt;i++)
if(triangles[i].link) // the real triangles
{
const Triangle &t=triangles[i];
// coor of 3 vertices
R2 A=t[0];
R2 B=t[1];
R2 C=t[2];
// number of the 3 vertices
iA = Number(t[0]);
iB = Number(t[1]);
iC = Number(t[2]);
Real8 dett = ::Area2(A,B,C);
detT[i]=dett;
dett /= 6;
// construction of on boundary
int nbb =0;
for(int j=0;j<3;j++)
{
Triangle *ta=t.Adj(j);
if ( ! ta || !ta->link) // no adj triangle => edge on boundary
OnBoundary[Number(t[VerticesOfTriangularEdge[j][0]])]=1,
OnBoundary[Number(t[VerticesOfTriangularEdge[j][1]])]=1,
nbb++;
}
workT[i] = nbb;
Mmass[iA] += dett;
Mmass[iB] += dett;
Mmass[iC] += dett;
if((nbb==0)|| !choice)
{
Mmassxx[iA] += dett;
Mmassxx[iB] += dett;
Mmassxx[iC] += dett;
}
}
else
workT[i]=-1;
for (Int4 kcount=0;kcount<n;kcount++,ss++)
{ //for all solution
Real8 smin=ss[0],smax=ss[0];
Real8 h1=1.e30,h2=1e-30,rx=0;
Real8 coef = 1./(anisomax*anisomax);
Real8 hn1=1.e30,hn2=1e-30,rnx =1.e-30;
for ( iv=0,k=0; iv<nbv; iv++,k+=n )
{
dxdx[iv]=dxdy[iv]=dydy[iv]=0;
smin=Min(smin,ss[k]);
smax=Max(smax,ss[k]);
}
Real8 sdelta = smax-smin;
Real8 absmax=Max(Abs(smin),Abs(smax));
Real8 cnorm = DoNormalisation ? coef2/sdelta : coef2;
if(verbosity>2)
cout << " Solution " << kcount << " Min = " << smin << " Max = "
<< smax << " Delta =" << sdelta << " cnorm = " << cnorm << endl;
if ( sdelta < 1.0e-10*Max(absmax,1e-20) )
{
if (verbosity>2)
cout << " Solution " << kcount << " is constant. We skip. "
<< " Min = " << smin << " Max = " << smax << endl;
continue;
}
for (i=0;i<nbt;i++)
if(triangles[i].link)
{// for real all triangles
// coor of 3 vertices
R2 A=triangles[i][0];
R2 B=triangles[i][1];
R2 C=triangles[i][2];
// warning the normal is internal and the
// size is the length of the edge
R2 nAB = Orthogonal(B-A);
R2 nBC = Orthogonal(C-B);
R2 nCA = Orthogonal(A-C);
// remark : nAB + nBC + nCA == 0
// number of the 3 vertices
iA = Number(triangles[i][0]);
iB = Number(triangles[i][1]);
iC = Number(triangles[i][2]);
// for the test of boundary edge
// the 3 adj triangles
Triangle *tBC = triangles[i].TriangleAdj(OppositeEdge[0]);
Triangle *tCA = triangles[i].TriangleAdj(OppositeEdge[1]);
Triangle *tAB = triangles[i].TriangleAdj(OppositeEdge[2]);
// value of the P1 fonction on 3 vertices
sA = ss[iA*n];
sB = ss[iB*n];
sC = ss[iC*n];
R2 Grads = (nAB * sC + nBC * sA + nCA * sB ) /detT[i] ;
if(choice)
{
int nbb = 0;
Real8 dd = detT[i];
Real8 lla,llb,llc,llf;
Real8 taa[3][3],bb[3];
// construction of the trans of lin system
for (int j=0;j<3;j++)
{
int ie = OppositeEdge[j];
TriangleAdjacent ta = triangles[i].Adj(ie);
Triangle *tt = ta;
if (tt && tt->link)
{
Vertex &v = *ta.OppositeVertex();
R2 V = v;
Int4 iV = Number(v);
Real8 lA = ::Area2(V,B,C)/dd;
Real8 lB = ::Area2(A,V,C)/dd;
Real8 lC = ::Area2(A,B,V)/dd;
taa[0][j] = lB*lC;
taa[1][j] = lC*lA;
taa[2][j] = lA*lB;
Real8 xx = V.x-V.y;
Real8 yy = V.x + V.y;
//cout << " iv " << ss[iV*n] << " == " << (8*xx*xx+yy*yy)
// << " l = " << lA << " " << lB << " " << lC
// << " = " << lA+lB+lC << " " << V << " == " << A*lA+B*lB+C*lC << endl;
lla = lA,llb=lB,llc=lC,llf=ss[iV*n] ;
bb[j] = ss[iV*n] - ( sA*lA + sB*lB + sC*lC ) ;
}
else
{
nbb++;
taa[0][j]=0;
taa[1][j]=0;
taa[2][j]=0;
taa[j][j]=1;
bb[j]=0;
}
}
// resolution of 3x3 lineaire system transpose
Real8 det33 = det3x3(taa[0],taa[1],taa[2]);
Real8 cBC = det3x3(bb,taa[1],taa[2]);
Real8 cCA = det3x3(taa[0],bb,taa[2]);
Real8 cAB = det3x3(taa[0],taa[1],bb);
assert(det33);
// det33=1;
// verif
// cout << " " << (taa[0][0]*cBC + taa[1][0]*cCA + taa[2][0] * cAB)/det33 << " == " << bb[0] ;
// cout << " " << (taa[0][1]*cBC + taa[1][1]*cCA + taa[2][1] * cAB)/det33 << " == " << bb[1];
// cout << " " << (taa[0][2]*cBC + taa[1][2]*cCA + taa[2][2] * cAB)/det33 << " == " << bb[2]
// << " -- " ;
//cout << lla*sA + llb*sB+llc*sC+ (lla*llb* cAB + llb*llc* cBC + llc*lla*cCA)/det33
// << " == " << llf << endl;
// computation of the gradient in the element
// H( li*lj) = grad li grad lj + grad lj grad lj
// grad li = njk / detT ; with i j k ={A,B,C)
Real8 Hxx = cAB * ( nBC.x*nCA.x) + cBC * ( nCA.x*nAB.x) + cCA * (nAB.x*nBC.x);
Real8 Hyy = cAB * ( nBC.y*nCA.y) + cBC * ( nCA.y*nAB.y) + cCA * (nAB.y*nBC.y);
Real8 Hxy = cAB * ( nBC.y*nCA.x) + cBC * ( nCA.y*nAB.x) + cCA * (nAB.y*nBC.x)
+ cAB * ( nBC.x*nCA.y) + cBC * ( nCA.x*nAB.y) + cCA * (nAB.x*nBC.y);
Real8 coef = 1.0/(3*dd*det33);
Real8 coef2 = 2*coef;
// cout << " H = " << Hxx << " " << Hyy << " " << Hxy/2 << " coef2 = " << coef2 << endl;
Hxx *= coef2;
Hyy *= coef2;
Hxy *= coef2;
//cout << i << " H = " << 3*Hxx/dd << " " << 3*Hyy/dd << " " << 3*Hxy/(dd*2) << " nbb = " << nbb << endl;
if(nbb==0)
{
dxdx[iA] += Hxx;
dydy[iA] += Hyy;
dxdy[iA] += Hxy;
dxdx[iB] += Hxx;
dydy[iB] += Hyy;
dxdy[iB] += Hxy;
dxdx[iC] += Hxx;
dydy[iC] += Hyy;
dxdy[iC] += Hxy;
}
}
else
{
// if edge on boundary no contribution => normal = 0
if ( ! tBC || ! tBC->link ) nBC = O;
if ( ! tCA || ! tCA->link ) nCA = O;
if ( ! tAB || ! tAB->link ) nAB = O;
// remark we forgot a 1/2 because
// \int_{edge} w_i = 1/2 if i is in edge
// 0 if not
// if we don't take the boundary
// dxdx[iA] += ( nCA.x + nAB.x ) *Grads.x;
dxdx[iA] += ( nCA.x + nAB.x ) *Grads.x;
dxdx[iB] += ( nAB.x + nBC.x ) *Grads.x;
dxdx[iC] += ( nBC.x + nCA.x ) *Grads.x;
// warning optimization (1) the divide by 2 is done on the metrix construction
dxdy[iA] += (( nCA.y + nAB.y ) *Grads.x + ( nCA.x + nAB.x ) *Grads.y) ;
dxdy[iB] += (( nAB.y + nBC.y ) *Grads.x + ( nAB.x + nBC.x ) *Grads.y) ;
dxdy[iC] += (( nBC.y + nCA.y ) *Grads.x + ( nBC.x + nCA.x ) *Grads.y) ;
dydy[iA] += ( nCA.y + nAB.y ) *Grads.y;
dydy[iB] += ( nAB.y + nBC.y ) *Grads.y;
dydy[iC] += ( nBC.y + nCA.y ) *Grads.y;
}
} // for real all triangles
Int4 kk=0;
for ( iv=0,k=0 ; iv<nbv; iv++,k+=n )
if(Mmassxx[iv]>0)
{
dxdx[iv] /= 2*Mmassxx[iv];
// warning optimization (1) on term dxdy[iv]*ci/2
dxdy[iv] /= 4*Mmassxx[iv];
dydy[iv] /= 2*Mmassxx[iv];
// Compute the matrix with abs(eigen value)
Metric M(dxdx[iv], dxdy[iv], dydy[iv]);
MatVVP2x2 Vp(M);
//cout <<iv << " M = " << M << " aniso= " << Vp.Aniso() ;
Vp.Abs();
M = Vp;
dxdx[iv] = M.a11;
dxdy[iv] = M.a21;
dydy[iv] = M.a22;
// cout << " (abs) iv M = " << M << " aniso= " << Vp.Aniso() <<endl;
}
else kk++;
// correction of second derivate
// by a laplacien
Real8 *d2[3] = { dxdx, dxdy, dydy};
Real8 *dd;
for (int xy = 0;xy<3;xy++)
{
dd = d2[xy];
// do leat 2 iteration for boundary problem
for (int ijacobi=0;ijacobi<Max(NbJacobi,2);ijacobi++)
{
for (i=0;i<nbt;i++)
if(triangles[i].link) // the real triangles
{
// number of the 3 vertices
iA = Number(triangles[i][0]);
iB = Number(triangles[i][1]);
iC = Number(triangles[i][2]);
Real8 cc=3;
if(ijacobi==0)
cc = Max((Real8) ((Mmassxx[iA]>0)+(Mmassxx[iB]>0)+(Mmassxx[iC]>0)),1.);
workT[i] = (dd[iA]+dd[iB]+dd[iC])/cc;
}
for (iv=0;iv<nbv;iv++)
workV[iv]=0;
for (i=0;i<nbt;i++)
if(triangles[i].link) // the real triangles
{
// number of the 3 vertices
iA = Number(triangles[i][0]);
iB = Number(triangles[i][1]);
iC = Number(triangles[i][2]);
Real8 cc = workT[i]*detT[i];
workV[iA] += cc;
workV[iB] += cc;
workV[iC] += cc;
}
for (iv=0;iv<nbv;iv++)
if( ijacobi<NbJacobi || OnBoundary[iv])
dd[iv] = workV[iv]/(Mmass[iv]*6);
}
}
// constuction of the metrix from the Hessian dxdx. dxdy,dydy
Real8 rCutOff=CutOff*absmax;// relative cut off
for ( iv=0,k=0 ; iv<nbv; iv++,k+=n )
{ // for all vertices
//{
//Metric M(dxdx[iv], dxdy[iv], dydy[iv]);
// MatVVP2x2 Vp(M);
//cout << " iv M="<< M << " Vp = " << Vp << " aniso " << Vp.Aniso() << endl;
//}
MetricIso Miso;
Real8 ci = RelativeMetric ? coef2/(Max(Abs(ss[k]),rCutOff)) : cnorm ;
Metric Miv(dxdx[iv]*ci, dxdy[iv]*ci, dydy[iv]*ci);
MatVVP2x2 Vp(Miv);
Vp.Abs();
h1=Min(h1,Vp.lmin());
h2=Max(h2,Vp.lmax());
Vp.Maxh(hmin);
Vp.Minh(hmax);
rx = Max(rx,Vp.Aniso2());
Vp.BoundAniso2(coef);
hn1=Min(hn1,Vp.lmin());
hn2=Max(hn2,Vp.lmax());
rnx = Max(rnx,Vp.Aniso2());
Metric MVp(Vp);
vertices[iv].m.IntersectWith(MVp);
}// for all vertices
if (verbosity>2)
{
cout << " Solution " << kcount << endl;
cout << " before bounding : Hmin = " << sqrt(1/h2) << " Hmax = "
<< sqrt(1/h1) << " factor of anisotropy max = " << sqrt(rx) << endl;
cout << " after bounding : Hmin = " << sqrt(1/hn2) << " Hmax = "
<< sqrt(1/hn1) << " factor of anisotropy max = " << sqrt(rnx) << endl;
}
}// end for all solution
delete [] detT;
delete [] Mmass;
delete [] dxdx;
delete [] dxdy;
delete [] dydy;
delete [] workT;
delete [] workV;
delete [] Mmassxx;
delete [] OnBoundary;
}
Plane::Plane(const std::vector<Point3d>& points)
: m_a(0.0), m_b(0.0), m_c(0.0), m_d(0.0)
{
unsigned N = points.size();
if (N < 3){
LOG_AND_THROW("Cannot compute plane with fewer than three points");
}else if (N == 3){
// duplicates code in point and normal ctor, points[1] is the point and (a x b) is the normal
Point3d point = points[1];
Vector3d a = points[1]-points[0];
Vector3d b = points[2]-points[1];
Vector3d thisNormal = a.cross(b);
if (!thisNormal.normalize()){
LOG_AND_THROW("Cannot initialize plane because normal is undefined");
}
m_a = thisNormal.x();
m_b = thisNormal.y();
m_c = thisNormal.z();
m_d = -thisNormal.x()*point.x() - thisNormal.y()*point.y() - thisNormal.z()*point.z();
}else{
bool foundSolution = false;
double tol = 1e-8; // 0.0001 was too big for the determinant tolerance, 1e-12 was too small
double maxDet = tol;
// solve the equation ax+by+cz+d=0 in a few different ways, keep the best one
{
// Ax = b, x = [a/c; b/c; d/c]
Matrix A(N,3);
Matrix At(3,N);
Vector b(N);
for (unsigned i = 0; i < N; ++i){
A(i,0) = points[i].x() - points[0].x();
A(i,1) = points[i].y() - points[0].y();
A(i,2) = 1.0;
At(0,i) = A(i,0);
At(1,i) = A(i,1);
At(2,i) = A(i,2);
b[i] = -(points[i].z() - points[0].z());
}
Matrix AtA = prod(At, A);
double det = det3x3(AtA); // always positive for A'*A
if (det > maxDet){
Matrix AtAInv(3,3);
bool test = invert(AtA, AtAInv);
if (test){
maxDet = det;
Vector x = prod(prod(AtAInv, At), b);
double a_c = x[0];
double b_c = x[1];
double d_c = x[2];
// a = a_c*c
// b = b_c*c
// d = d_c*c
// a^2 + b^2 + c^2 = 1
// c^2*(a_c^2 + b_c^2 + 1) = 1
m_c = 1.0/sqrt(a_c*a_c + b_c*b_c + 1.0);
m_a = a_c*m_c;
m_b = b_c*m_c;
m_d = d_c*m_c - m_a*points[0].x() - m_b*points[0].y() - m_c*points[0].z();
foundSolution = true;
}
}
}
{
// Ax = b, x = [a/b; c/b; d/b]
Matrix A(N,3);
Matrix At(3,N);
Vector b(N);
for (unsigned i = 0; i < N; ++i){
A(i,0) = points[i].x() - points[0].x();
A(i,1) = points[i].z() - points[0].z();
A(i,2) = 1.0;
At(0,i) = A(i,0);
At(1,i) = A(i,1);
At(2,i) = A(i,2);
b[i] = -(points[i].y() - points[0].y());
}
Matrix AtA = prod(At, A);
double det = det3x3(AtA); // always positive for A'*A
if (det > maxDet){
Matrix AtAInv(3,3);
bool test = invert(AtA, AtAInv);
if (test){
maxDet = det;
Vector x = prod(prod(AtAInv, At), b);
double a_b = x[0];
double c_b = x[1];
double d_b = x[2];
// a = a_b*b
// c = c_b*b
// d = d_b*b
// a^2 + b^2 + c^2 = 1
// b^2*(a_b^2 + c_b^2 + 1) = 1
m_b = 1.0/sqrt(a_b*a_b + c_b*c_b + 1.0);
m_a = a_b*m_b;
m_c = c_b*m_b;
m_d = d_b*m_b - m_a*points[0].x() - m_b*points[0].y() - m_c*points[0].z();
foundSolution = true;
}
}
}
{
// Ax = b, x = [b/a; c/a; d/a]
Matrix A(N,3);
Matrix At(3,N);
Vector b(N);
for (unsigned i = 0; i < N; ++i){
A(i,0) = points[i].y() - points[0].y();
A(i,1) = points[i].z() - points[0].z();
A(i,2) = 1.0;
At(0,i) = A(i,0);
At(1,i) = A(i,1);
At(2,i) = A(i,2);
b[i] = -(points[i].x() - points[0].x());
}
Matrix AtA = prod(At, A);
double det = det3x3(AtA); // always positive for A'*A
if (det > maxDet){
Matrix AtAInv(3,3);
bool test = invert(AtA, AtAInv);
if (test){
maxDet = det;
Vector x = prod(prod(AtAInv, At), b);
double b_a = x[0];
double c_a = x[1];
double d_a = x[2];
// b = b_a*a
// c = c_a*a
// d = d_a*a
// a^2 + b^2 + c^2 = 1
// a^2*(b_a^2 + c_a^2 + 1) = 1
m_a = 1.0/sqrt(b_a*b_a + c_a*c_a + 1.0);
m_b = b_a*m_a;
m_c = c_a*m_a;
m_d = d_a*m_a - m_a*points[0].x() - m_b*points[0].y() - m_c*points[0].z();
foundSolution = true;
}
}
}
if (!foundSolution){
//std::stringstream ss;
//ss << std::fixed << std::setprecision(20) << points << std::endl;
//std::string s = ss.str();
LOG_AND_THROW("Cannot compute plane for points " << points);
}
// get outward normal from vertices, plane outward normal should match sense of vertices
// this corresponds to the other solution to the sqrt
boost::optional<Vector3d> outwardNormal = getOutwardNormal(points);
if (outwardNormal){
double dot = m_a*outwardNormal.get().x() + m_b*outwardNormal.get().y() + m_c*outwardNormal.get().z();
if (dot < 0){
m_a = -m_a;
m_b = -m_b;
m_c = -m_c;
m_d = -m_d;
}
}
// test that normal has length 1
double length = m_a*m_a + m_b*m_b + m_c*m_c;
tol = 0.0001;
OS_ASSERT(fabs(1.0-length) <= tol);
}
}
int Matrix::Inverse(Matrix &m, float epsilon) const {
m = *this;
float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
a1 = m.data[0][0]; b1 = m.data[0][1]; c1 = m.data[0][2]; d1 = m.data[0][3];
a2 = m.data[1][0]; b2 = m.data[1][1]; c2 = m.data[1][2]; d2 = m.data[1][3];
a3 = m.data[2][0]; b3 = m.data[2][1]; c3 = m.data[2][2]; d3 = m.data[2][3];
a4 = m.data[3][0]; b4 = m.data[3][1]; c4 = m.data[3][2]; d4 = m.data[3][3];
float det = det4x4(a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4);
if (fabs(det) < epsilon) {
printf ("Matrix::Inverse --- singular matrix, can't invert!\n");
assert(0);
return 0;
}
m.data[0][0] = det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
m.data[1][0] = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
m.data[2][0] = det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
m.data[3][0] = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
m.data[0][1] = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
m.data[1][1] = det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
m.data[2][1] = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
m.data[3][1] = det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
m.data[0][2] = det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
m.data[1][2] = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
m.data[2][2] = det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
m.data[3][2] = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
m.data[0][3] = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
m.data[1][3] = det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
m.data[2][3] = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
m.data[3][3] = det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
m *= 1/det;
return 1;
}
void adjoint (Matrix mat)
{
double a1, a2, a3, a4, b1, b2, b3, b4;
double c1, c2, c3, c4, d1, d2, d3, d4;
a1 = mat[0][0];
b1 = mat[0][1];
c1 = mat[0][2];
d1 = mat[0][3];
a2 = mat[1][0];
b2 = mat[1][1];
c2 = mat[1][2];
d2 = mat[1][3];
a3 = mat[2][0];
b3 = mat[2][1];
c3 = mat[2][2];
d3 = mat[2][3];
a4 = mat[3][0];
b4 = mat[3][1];
c4 = mat[3][2];
d4 = mat[3][3];
/* row column labeling reversed since we transpose rows & columns */
mat[0][0] = det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
mat[1][0] = -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
mat[2][0] = det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
mat[3][0] = -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
mat[0][1] = -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
mat[1][1] = det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
mat[2][1] = -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
mat[3][1] = det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
mat[0][2] = det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
mat[1][2] = -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
mat[2][2] = det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
mat[3][2] = -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
mat[0][3] = -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
mat[1][3] = det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
mat[2][3] = -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
mat[3][3] = det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
}
/*!
* Find the adjoint of a matrix.
*
* \ingroup matrix
*/
void
lib3ds_matrix_adjoint(Lib3dsMatrix m)
{
Lib3dsFloat a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
a1 = m[0][0];
b1 = m[1][0];
c1 = m[2][0];
d1 = m[3][0];
a2 = m[0][1];
b2 = m[1][1];
c2 = m[2][1];
d2 = m[3][1];
a3 = m[0][2];
b3 = m[1][2];
c3 = m[2][2];
d3 = m[3][2];
a4 = m[0][3];
b4 = m[1][3];
c4 = m[2][3];
d4 = m[3][3];
m[0][0]= det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
m[0][1]= -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
m[0][2]= det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
m[0][3]= -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
m[1][0]= -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
m[1][1]= det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
m[1][2]= -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
m[1][3]= det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
m[2][0]= det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
m[2][1]= -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
m[2][2]= det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
m[2][3]= -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
m[3][0]= -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
m[3][1]= det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
m[3][2]= -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
m[3][3]= det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
}
nv_scalar det(const mat3& A)
{
return det3x3(A.a00, A.a01, A.a02,
A.a10, A.a11, A.a12,
A.a20, A.a21, A.a22);
}
