Code-style consistency improvement:
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
@@ -20,39 +20,43 @@ subject to the following restrictions:
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#include "LinearR4.h"
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
const VectorR4 VectorR4::Zero(0.0, 0.0, 0.0, 0.0);
|
||||
const VectorR4 VectorR4::UnitX( 1.0, 0.0, 0.0, 0.0);
|
||||
const VectorR4 VectorR4::UnitY( 0.0, 1.0, 0.0, 0.0);
|
||||
const VectorR4 VectorR4::UnitZ( 0.0, 0.0, 1.0, 0.0);
|
||||
const VectorR4 VectorR4::UnitW( 0.0, 0.0, 0.0, 1.0);
|
||||
const VectorR4 VectorR4::UnitX(1.0, 0.0, 0.0, 0.0);
|
||||
const VectorR4 VectorR4::UnitY(0.0, 1.0, 0.0, 0.0);
|
||||
const VectorR4 VectorR4::UnitZ(0.0, 0.0, 1.0, 0.0);
|
||||
const VectorR4 VectorR4::UnitW(0.0, 0.0, 0.0, 1.0);
|
||||
const VectorR4 VectorR4::NegUnitX(-1.0, 0.0, 0.0, 0.0);
|
||||
const VectorR4 VectorR4::NegUnitY( 0.0,-1.0, 0.0, 0.0);
|
||||
const VectorR4 VectorR4::NegUnitZ( 0.0, 0.0,-1.0, 0.0);
|
||||
const VectorR4 VectorR4::NegUnitW( 0.0, 0.0, 0.0,-1.0);
|
||||
const VectorR4 VectorR4::NegUnitY(0.0, -1.0, 0.0, 0.0);
|
||||
const VectorR4 VectorR4::NegUnitZ(0.0, 0.0, -1.0, 0.0);
|
||||
const VectorR4 VectorR4::NegUnitW(0.0, 0.0, 0.0, -1.0);
|
||||
|
||||
const Matrix4x4 Matrix4x4::Identity(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
|
||||
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
|
||||
|
||||
|
||||
// ******************************************************
|
||||
// * VectorR4 class - math library functions *
|
||||
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
||||
|
||||
double VectorR4::MaxAbs() const
|
||||
{
|
||||
double m;
|
||||
m = (x>0.0) ? x : -x;
|
||||
if ( y>m ) m=y;
|
||||
else if ( -y >m ) m = -y;
|
||||
if ( z>m ) m=z;
|
||||
else if ( -z>m ) m = -z;
|
||||
if ( w>m ) m=w;
|
||||
else if ( -w>m ) m = -w;
|
||||
double m;
|
||||
m = (x > 0.0) ? x : -x;
|
||||
if (y > m)
|
||||
m = y;
|
||||
else if (-y > m)
|
||||
m = -y;
|
||||
if (z > m)
|
||||
m = z;
|
||||
else if (-z > m)
|
||||
m = -z;
|
||||
if (w > m)
|
||||
m = w;
|
||||
else if (-w > m)
|
||||
m = -w;
|
||||
return m;
|
||||
}
|
||||
|
||||
@@ -60,90 +64,90 @@ double VectorR4::MaxAbs() const
|
||||
// * Matrix4x4 class - math library functions *
|
||||
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
||||
|
||||
|
||||
void Matrix4x4::operator*= (const Matrix4x4& B) // Matrix product
|
||||
void Matrix4x4::operator*=(const Matrix4x4& B) // Matrix product
|
||||
{
|
||||
double t1, t2, t3; // temporary values
|
||||
t1 = m11*B.m11 + m12*B.m21 + m13*B.m31 + m14*B.m41;
|
||||
t2 = m11*B.m12 + m12*B.m22 + m13*B.m32 + m14*B.m42;
|
||||
t3 = m11*B.m13 + m12*B.m23 + m13*B.m33 + m14*B.m43;
|
||||
m14 = m11*B.m14 + m12*B.m24 + m13*B.m34 + m14*B.m44;
|
||||
double t1, t2, t3; // temporary values
|
||||
t1 = m11 * B.m11 + m12 * B.m21 + m13 * B.m31 + m14 * B.m41;
|
||||
t2 = m11 * B.m12 + m12 * B.m22 + m13 * B.m32 + m14 * B.m42;
|
||||
t3 = m11 * B.m13 + m12 * B.m23 + m13 * B.m33 + m14 * B.m43;
|
||||
m14 = m11 * B.m14 + m12 * B.m24 + m13 * B.m34 + m14 * B.m44;
|
||||
m11 = t1;
|
||||
m12 = t2;
|
||||
m13 = t3;
|
||||
|
||||
t1 = m21*B.m11 + m22*B.m21 + m23*B.m31 + m24*B.m41;
|
||||
t2 = m21*B.m12 + m22*B.m22 + m23*B.m32 + m24*B.m42;
|
||||
t3 = m21*B.m13 + m22*B.m23 + m23*B.m33 + m24*B.m43;
|
||||
m24 = m21*B.m14 + m22*B.m24 + m23*B.m34 + m24*B.m44;
|
||||
t1 = m21 * B.m11 + m22 * B.m21 + m23 * B.m31 + m24 * B.m41;
|
||||
t2 = m21 * B.m12 + m22 * B.m22 + m23 * B.m32 + m24 * B.m42;
|
||||
t3 = m21 * B.m13 + m22 * B.m23 + m23 * B.m33 + m24 * B.m43;
|
||||
m24 = m21 * B.m14 + m22 * B.m24 + m23 * B.m34 + m24 * B.m44;
|
||||
m21 = t1;
|
||||
m22 = t2;
|
||||
m23 = t3;
|
||||
|
||||
t1 = m31*B.m11 + m32*B.m21 + m33*B.m31 + m34*B.m41;
|
||||
t2 = m31*B.m12 + m32*B.m22 + m33*B.m32 + m34*B.m42;
|
||||
t3 = m31*B.m13 + m32*B.m23 + m33*B.m33 + m34*B.m43;
|
||||
m34 = m31*B.m14 + m32*B.m24 + m33*B.m34 + m34*B.m44;
|
||||
t1 = m31 * B.m11 + m32 * B.m21 + m33 * B.m31 + m34 * B.m41;
|
||||
t2 = m31 * B.m12 + m32 * B.m22 + m33 * B.m32 + m34 * B.m42;
|
||||
t3 = m31 * B.m13 + m32 * B.m23 + m33 * B.m33 + m34 * B.m43;
|
||||
m34 = m31 * B.m14 + m32 * B.m24 + m33 * B.m34 + m34 * B.m44;
|
||||
m31 = t1;
|
||||
m32 = t2;
|
||||
m33 = t3;
|
||||
|
||||
t1 = m41*B.m11 + m42*B.m21 + m43*B.m31 + m44*B.m41;
|
||||
t2 = m41*B.m12 + m42*B.m22 + m43*B.m32 + m44*B.m42;
|
||||
t3 = m41*B.m13 + m42*B.m23 + m43*B.m33 + m44*B.m43;
|
||||
m44 = m41*B.m14 + m42*B.m24 + m43*B.m34 + m44*B.m44;
|
||||
t1 = m41 * B.m11 + m42 * B.m21 + m43 * B.m31 + m44 * B.m41;
|
||||
t2 = m41 * B.m12 + m42 * B.m22 + m43 * B.m32 + m44 * B.m42;
|
||||
t3 = m41 * B.m13 + m42 * B.m23 + m43 * B.m33 + m44 * B.m43;
|
||||
m44 = m41 * B.m14 + m42 * B.m24 + m43 * B.m34 + m44 * B.m44;
|
||||
m41 = t1;
|
||||
m42 = t2;
|
||||
m43 = t3;
|
||||
}
|
||||
|
||||
inline void ReNormalizeHelper ( double &a, double &b, double &c, double &d )
|
||||
inline void ReNormalizeHelper(double& a, double& b, double& c, double& d)
|
||||
{
|
||||
double scaleF = a*a+b*b+c*c+d*d; // Inner product of Vector-R4
|
||||
scaleF = 1.0-0.5*(scaleF-1.0);
|
||||
double scaleF = a * a + b * b + c * c + d * d; // Inner product of Vector-R4
|
||||
scaleF = 1.0 - 0.5 * (scaleF - 1.0);
|
||||
a *= scaleF;
|
||||
b *= scaleF;
|
||||
c *= scaleF;
|
||||
d *= scaleF;
|
||||
}
|
||||
|
||||
Matrix4x4& Matrix4x4::ReNormalize() {
|
||||
ReNormalizeHelper( m11, m21, m31, m41 ); // Renormalize first column
|
||||
ReNormalizeHelper( m12, m22, m32, m42 ); // Renormalize second column
|
||||
ReNormalizeHelper( m13, m23, m33, m43 ); // Renormalize third column
|
||||
ReNormalizeHelper( m14, m24, m34, m44 ); // Renormalize fourth column
|
||||
double alpha = 0.5*(m11*m12 + m21*m22 + m31*m32 + m41*m42); //1st and 2nd cols
|
||||
double beta = 0.5*(m11*m13 + m21*m23 + m31*m33 + m41*m43); //1st and 3rd cols
|
||||
double gamma = 0.5*(m11*m14 + m21*m24 + m31*m34 + m41*m44); //1st and 4nd cols
|
||||
double delta = 0.5*(m12*m13 + m22*m23 + m32*m33 + m42*m43); //2nd and 3rd cols
|
||||
double eps = 0.5*(m12*m14 + m22*m24 + m32*m34 + m42*m44); //2nd and 4nd cols
|
||||
double phi = 0.5*(m13*m14 + m23*m24 + m33*m34 + m43*m44); //3rd and 4nd cols
|
||||
Matrix4x4& Matrix4x4::ReNormalize()
|
||||
{
|
||||
ReNormalizeHelper(m11, m21, m31, m41); // Renormalize first column
|
||||
ReNormalizeHelper(m12, m22, m32, m42); // Renormalize second column
|
||||
ReNormalizeHelper(m13, m23, m33, m43); // Renormalize third column
|
||||
ReNormalizeHelper(m14, m24, m34, m44); // Renormalize fourth column
|
||||
double alpha = 0.5 * (m11 * m12 + m21 * m22 + m31 * m32 + m41 * m42); //1st and 2nd cols
|
||||
double beta = 0.5 * (m11 * m13 + m21 * m23 + m31 * m33 + m41 * m43); //1st and 3rd cols
|
||||
double gamma = 0.5 * (m11 * m14 + m21 * m24 + m31 * m34 + m41 * m44); //1st and 4nd cols
|
||||
double delta = 0.5 * (m12 * m13 + m22 * m23 + m32 * m33 + m42 * m43); //2nd and 3rd cols
|
||||
double eps = 0.5 * (m12 * m14 + m22 * m24 + m32 * m34 + m42 * m44); //2nd and 4nd cols
|
||||
double phi = 0.5 * (m13 * m14 + m23 * m24 + m33 * m34 + m43 * m44); //3rd and 4nd cols
|
||||
double temp1, temp2, temp3;
|
||||
temp1 = m11 - alpha*m12 - beta*m13 - gamma*m14;
|
||||
temp2 = m12 - alpha*m11 - delta*m13 - eps*m14;
|
||||
temp3 = m13 - beta*m11 - delta*m12 - phi*m14;
|
||||
m14 -= (gamma*m11 + eps*m12 + phi*m13);
|
||||
temp1 = m11 - alpha * m12 - beta * m13 - gamma * m14;
|
||||
temp2 = m12 - alpha * m11 - delta * m13 - eps * m14;
|
||||
temp3 = m13 - beta * m11 - delta * m12 - phi * m14;
|
||||
m14 -= (gamma * m11 + eps * m12 + phi * m13);
|
||||
m11 = temp1;
|
||||
m12 = temp2;
|
||||
m13 = temp3;
|
||||
temp1 = m21 - alpha*m22 - beta*m23 - gamma*m24;
|
||||
temp2 = m22 - alpha*m21 - delta*m23 - eps*m24;
|
||||
temp3 = m23 - beta*m21 - delta*m22 - phi*m24;
|
||||
m24 -= (gamma*m21 + eps*m22 + phi*m23);
|
||||
temp1 = m21 - alpha * m22 - beta * m23 - gamma * m24;
|
||||
temp2 = m22 - alpha * m21 - delta * m23 - eps * m24;
|
||||
temp3 = m23 - beta * m21 - delta * m22 - phi * m24;
|
||||
m24 -= (gamma * m21 + eps * m22 + phi * m23);
|
||||
m21 = temp1;
|
||||
m22 = temp2;
|
||||
m23 = temp3;
|
||||
temp1 = m31 - alpha*m32 - beta*m33 - gamma*m34;
|
||||
temp2 = m32 - alpha*m31 - delta*m33 - eps*m34;
|
||||
temp3 = m33 - beta*m31 - delta*m32 - phi*m34;
|
||||
m34 -= (gamma*m31 + eps*m32 + phi*m33);
|
||||
temp1 = m31 - alpha * m32 - beta * m33 - gamma * m34;
|
||||
temp2 = m32 - alpha * m31 - delta * m33 - eps * m34;
|
||||
temp3 = m33 - beta * m31 - delta * m32 - phi * m34;
|
||||
m34 -= (gamma * m31 + eps * m32 + phi * m33);
|
||||
m31 = temp1;
|
||||
m32 = temp2;
|
||||
m33 = temp3;
|
||||
temp1 = m41 - alpha*m42 - beta*m43 - gamma*m44;
|
||||
temp2 = m42 - alpha*m41 - delta*m43 - eps*m44;
|
||||
temp3 = m43 - beta*m41 - delta*m42 - phi*m44;
|
||||
m44 -= (gamma*m41 + eps*m42 + phi*m43);
|
||||
temp1 = m41 - alpha * m42 - beta * m43 - gamma * m44;
|
||||
temp2 = m42 - alpha * m41 - delta * m43 - eps * m44;
|
||||
temp3 = m43 - beta * m41 - delta * m42 - phi * m44;
|
||||
m44 -= (gamma * m41 + eps * m42 + phi * m43);
|
||||
m41 = temp1;
|
||||
m42 = temp2;
|
||||
m43 = temp3;
|
||||
@@ -154,223 +158,221 @@ Matrix4x4& Matrix4x4::ReNormalize() {
|
||||
// * LinearMapR4 class - math library functions *
|
||||
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
||||
|
||||
|
||||
double LinearMapR4::Determinant () const // Returns the determinant
|
||||
double LinearMapR4::Determinant() const // Returns the determinant
|
||||
{
|
||||
double Tbt34C12 = m31*m42-m32*m41; // 2x2 subdeterminants
|
||||
double Tbt34C13 = m31*m43-m33*m41;
|
||||
double Tbt34C14 = m31*m44-m34*m41;
|
||||
double Tbt34C23 = m32*m43-m33*m42;
|
||||
double Tbt34C24 = m32*m44-m34*m42;
|
||||
double Tbt34C34 = m33*m44-m34*m43;
|
||||
double Tbt34C12 = m31 * m42 - m32 * m41; // 2x2 subdeterminants
|
||||
double Tbt34C13 = m31 * m43 - m33 * m41;
|
||||
double Tbt34C14 = m31 * m44 - m34 * m41;
|
||||
double Tbt34C23 = m32 * m43 - m33 * m42;
|
||||
double Tbt34C24 = m32 * m44 - m34 * m42;
|
||||
double Tbt34C34 = m33 * m44 - m34 * m43;
|
||||
|
||||
double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23; // 3x3 subdeterminants
|
||||
double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13;
|
||||
double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12;
|
||||
double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12;
|
||||
double sd11 = m22 * Tbt34C34 - m23 * Tbt34C24 + m24 * Tbt34C23; // 3x3 subdeterminants
|
||||
double sd12 = m21 * Tbt34C34 - m23 * Tbt34C14 + m24 * Tbt34C13;
|
||||
double sd13 = m21 * Tbt34C24 - m22 * Tbt34C14 + m24 * Tbt34C12;
|
||||
double sd14 = m21 * Tbt34C23 - m22 * Tbt34C13 + m23 * Tbt34C12;
|
||||
|
||||
return ( m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14 );
|
||||
return (m11 * sd11 - m12 * sd12 + m13 * sd13 - m14 * sd14);
|
||||
}
|
||||
|
||||
LinearMapR4 LinearMapR4::Inverse() const // Returns inverse
|
||||
LinearMapR4 LinearMapR4::Inverse() const // Returns inverse
|
||||
{
|
||||
double Tbt34C12 = m31 * m42 - m32 * m41; // 2x2 subdeterminants
|
||||
double Tbt34C13 = m31 * m43 - m33 * m41;
|
||||
double Tbt34C14 = m31 * m44 - m34 * m41;
|
||||
double Tbt34C23 = m32 * m43 - m33 * m42;
|
||||
double Tbt34C24 = m32 * m44 - m34 * m42;
|
||||
double Tbt34C34 = m33 * m44 - m34 * m43;
|
||||
double Tbt24C12 = m21 * m42 - m22 * m41; // 2x2 subdeterminants
|
||||
double Tbt24C13 = m21 * m43 - m23 * m41;
|
||||
double Tbt24C14 = m21 * m44 - m24 * m41;
|
||||
double Tbt24C23 = m22 * m43 - m23 * m42;
|
||||
double Tbt24C24 = m22 * m44 - m24 * m42;
|
||||
double Tbt24C34 = m23 * m44 - m24 * m43;
|
||||
double Tbt23C12 = m21 * m32 - m22 * m31; // 2x2 subdeterminants
|
||||
double Tbt23C13 = m21 * m33 - m23 * m31;
|
||||
double Tbt23C14 = m21 * m34 - m24 * m31;
|
||||
double Tbt23C23 = m22 * m33 - m23 * m32;
|
||||
double Tbt23C24 = m22 * m34 - m24 * m32;
|
||||
double Tbt23C34 = m23 * m34 - m24 * m33;
|
||||
|
||||
double Tbt34C12 = m31*m42-m32*m41; // 2x2 subdeterminants
|
||||
double Tbt34C13 = m31*m43-m33*m41;
|
||||
double Tbt34C14 = m31*m44-m34*m41;
|
||||
double Tbt34C23 = m32*m43-m33*m42;
|
||||
double Tbt34C24 = m32*m44-m34*m42;
|
||||
double Tbt34C34 = m33*m44-m34*m43;
|
||||
double Tbt24C12 = m21*m42-m22*m41; // 2x2 subdeterminants
|
||||
double Tbt24C13 = m21*m43-m23*m41;
|
||||
double Tbt24C14 = m21*m44-m24*m41;
|
||||
double Tbt24C23 = m22*m43-m23*m42;
|
||||
double Tbt24C24 = m22*m44-m24*m42;
|
||||
double Tbt24C34 = m23*m44-m24*m43;
|
||||
double Tbt23C12 = m21*m32-m22*m31; // 2x2 subdeterminants
|
||||
double Tbt23C13 = m21*m33-m23*m31;
|
||||
double Tbt23C14 = m21*m34-m24*m31;
|
||||
double Tbt23C23 = m22*m33-m23*m32;
|
||||
double Tbt23C24 = m22*m34-m24*m32;
|
||||
double Tbt23C34 = m23*m34-m24*m33;
|
||||
double sd11 = m22 * Tbt34C34 - m23 * Tbt34C24 + m24 * Tbt34C23; // 3x3 subdeterminants
|
||||
double sd12 = m21 * Tbt34C34 - m23 * Tbt34C14 + m24 * Tbt34C13;
|
||||
double sd13 = m21 * Tbt34C24 - m22 * Tbt34C14 + m24 * Tbt34C12;
|
||||
double sd14 = m21 * Tbt34C23 - m22 * Tbt34C13 + m23 * Tbt34C12;
|
||||
double sd21 = m12 * Tbt34C34 - m13 * Tbt34C24 + m14 * Tbt34C23; // 3x3 subdeterminants
|
||||
double sd22 = m11 * Tbt34C34 - m13 * Tbt34C14 + m14 * Tbt34C13;
|
||||
double sd23 = m11 * Tbt34C24 - m12 * Tbt34C14 + m14 * Tbt34C12;
|
||||
double sd24 = m11 * Tbt34C23 - m12 * Tbt34C13 + m13 * Tbt34C12;
|
||||
double sd31 = m12 * Tbt24C34 - m13 * Tbt24C24 + m14 * Tbt24C23; // 3x3 subdeterminants
|
||||
double sd32 = m11 * Tbt24C34 - m13 * Tbt24C14 + m14 * Tbt24C13;
|
||||
double sd33 = m11 * Tbt24C24 - m12 * Tbt24C14 + m14 * Tbt24C12;
|
||||
double sd34 = m11 * Tbt24C23 - m12 * Tbt24C13 + m13 * Tbt24C12;
|
||||
double sd41 = m12 * Tbt23C34 - m13 * Tbt23C24 + m14 * Tbt23C23; // 3x3 subdeterminants
|
||||
double sd42 = m11 * Tbt23C34 - m13 * Tbt23C14 + m14 * Tbt23C13;
|
||||
double sd43 = m11 * Tbt23C24 - m12 * Tbt23C14 + m14 * Tbt23C12;
|
||||
double sd44 = m11 * Tbt23C23 - m12 * Tbt23C13 + m13 * Tbt23C12;
|
||||
|
||||
double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23; // 3x3 subdeterminants
|
||||
double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13;
|
||||
double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12;
|
||||
double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12;
|
||||
double sd21 = m12*Tbt34C34 - m13*Tbt34C24 + m14*Tbt34C23; // 3x3 subdeterminants
|
||||
double sd22 = m11*Tbt34C34 - m13*Tbt34C14 + m14*Tbt34C13;
|
||||
double sd23 = m11*Tbt34C24 - m12*Tbt34C14 + m14*Tbt34C12;
|
||||
double sd24 = m11*Tbt34C23 - m12*Tbt34C13 + m13*Tbt34C12;
|
||||
double sd31 = m12*Tbt24C34 - m13*Tbt24C24 + m14*Tbt24C23; // 3x3 subdeterminants
|
||||
double sd32 = m11*Tbt24C34 - m13*Tbt24C14 + m14*Tbt24C13;
|
||||
double sd33 = m11*Tbt24C24 - m12*Tbt24C14 + m14*Tbt24C12;
|
||||
double sd34 = m11*Tbt24C23 - m12*Tbt24C13 + m13*Tbt24C12;
|
||||
double sd41 = m12*Tbt23C34 - m13*Tbt23C24 + m14*Tbt23C23; // 3x3 subdeterminants
|
||||
double sd42 = m11*Tbt23C34 - m13*Tbt23C14 + m14*Tbt23C13;
|
||||
double sd43 = m11*Tbt23C24 - m12*Tbt23C14 + m14*Tbt23C12;
|
||||
double sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12;
|
||||
double detInv = 1.0 / (m11 * sd11 - m12 * sd12 + m13 * sd13 - m14 * sd14);
|
||||
|
||||
|
||||
double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14);
|
||||
|
||||
return( LinearMapR4( sd11*detInv, -sd12*detInv, sd13*detInv, -sd14*detInv,
|
||||
-sd21*detInv, sd22*detInv, -sd23*detInv, sd24*detInv,
|
||||
sd31*detInv, -sd32*detInv, sd33*detInv, -sd34*detInv,
|
||||
-sd41*detInv, sd42*detInv, -sd43*detInv, sd44*detInv ) );
|
||||
return (LinearMapR4(sd11 * detInv, -sd12 * detInv, sd13 * detInv, -sd14 * detInv,
|
||||
-sd21 * detInv, sd22 * detInv, -sd23 * detInv, sd24 * detInv,
|
||||
sd31 * detInv, -sd32 * detInv, sd33 * detInv, -sd34 * detInv,
|
||||
-sd41 * detInv, sd42 * detInv, -sd43 * detInv, sd44 * detInv));
|
||||
}
|
||||
|
||||
LinearMapR4& LinearMapR4::Invert() // Converts into inverse.
|
||||
LinearMapR4& LinearMapR4::Invert() // Converts into inverse.
|
||||
{
|
||||
double Tbt34C12 = m31*m42-m32*m41; // 2x2 subdeterminants
|
||||
double Tbt34C13 = m31*m43-m33*m41;
|
||||
double Tbt34C14 = m31*m44-m34*m41;
|
||||
double Tbt34C23 = m32*m43-m33*m42;
|
||||
double Tbt34C24 = m32*m44-m34*m42;
|
||||
double Tbt34C34 = m33*m44-m34*m43;
|
||||
double Tbt24C12 = m21*m42-m22*m41; // 2x2 subdeterminants
|
||||
double Tbt24C13 = m21*m43-m23*m41;
|
||||
double Tbt24C14 = m21*m44-m24*m41;
|
||||
double Tbt24C23 = m22*m43-m23*m42;
|
||||
double Tbt24C24 = m22*m44-m24*m42;
|
||||
double Tbt24C34 = m23*m44-m24*m43;
|
||||
double Tbt23C12 = m21*m32-m22*m31; // 2x2 subdeterminants
|
||||
double Tbt23C13 = m21*m33-m23*m31;
|
||||
double Tbt23C14 = m21*m34-m24*m31;
|
||||
double Tbt23C23 = m22*m33-m23*m32;
|
||||
double Tbt23C24 = m22*m34-m24*m32;
|
||||
double Tbt23C34 = m23*m34-m24*m33;
|
||||
double Tbt34C12 = m31 * m42 - m32 * m41; // 2x2 subdeterminants
|
||||
double Tbt34C13 = m31 * m43 - m33 * m41;
|
||||
double Tbt34C14 = m31 * m44 - m34 * m41;
|
||||
double Tbt34C23 = m32 * m43 - m33 * m42;
|
||||
double Tbt34C24 = m32 * m44 - m34 * m42;
|
||||
double Tbt34C34 = m33 * m44 - m34 * m43;
|
||||
double Tbt24C12 = m21 * m42 - m22 * m41; // 2x2 subdeterminants
|
||||
double Tbt24C13 = m21 * m43 - m23 * m41;
|
||||
double Tbt24C14 = m21 * m44 - m24 * m41;
|
||||
double Tbt24C23 = m22 * m43 - m23 * m42;
|
||||
double Tbt24C24 = m22 * m44 - m24 * m42;
|
||||
double Tbt24C34 = m23 * m44 - m24 * m43;
|
||||
double Tbt23C12 = m21 * m32 - m22 * m31; // 2x2 subdeterminants
|
||||
double Tbt23C13 = m21 * m33 - m23 * m31;
|
||||
double Tbt23C14 = m21 * m34 - m24 * m31;
|
||||
double Tbt23C23 = m22 * m33 - m23 * m32;
|
||||
double Tbt23C24 = m22 * m34 - m24 * m32;
|
||||
double Tbt23C34 = m23 * m34 - m24 * m33;
|
||||
|
||||
double sd11 = m22*Tbt34C34 - m23*Tbt34C24 + m24*Tbt34C23; // 3x3 subdeterminants
|
||||
double sd12 = m21*Tbt34C34 - m23*Tbt34C14 + m24*Tbt34C13;
|
||||
double sd13 = m21*Tbt34C24 - m22*Tbt34C14 + m24*Tbt34C12;
|
||||
double sd14 = m21*Tbt34C23 - m22*Tbt34C13 + m23*Tbt34C12;
|
||||
double sd21 = m12*Tbt34C34 - m13*Tbt34C24 + m14*Tbt34C23; // 3x3 subdeterminants
|
||||
double sd22 = m11*Tbt34C34 - m13*Tbt34C14 + m14*Tbt34C13;
|
||||
double sd23 = m11*Tbt34C24 - m12*Tbt34C14 + m14*Tbt34C12;
|
||||
double sd24 = m11*Tbt34C23 - m12*Tbt34C13 + m13*Tbt34C12;
|
||||
double sd31 = m12*Tbt24C34 - m13*Tbt24C24 + m14*Tbt24C23; // 3x3 subdeterminants
|
||||
double sd32 = m11*Tbt24C34 - m13*Tbt24C14 + m14*Tbt24C13;
|
||||
double sd33 = m11*Tbt24C24 - m12*Tbt24C14 + m14*Tbt24C12;
|
||||
double sd34 = m11*Tbt24C23 - m12*Tbt24C13 + m13*Tbt24C12;
|
||||
double sd41 = m12*Tbt23C34 - m13*Tbt23C24 + m14*Tbt23C23; // 3x3 subdeterminants
|
||||
double sd42 = m11*Tbt23C34 - m13*Tbt23C14 + m14*Tbt23C13;
|
||||
double sd43 = m11*Tbt23C24 - m12*Tbt23C14 + m14*Tbt23C12;
|
||||
double sd44 = m11*Tbt23C23 - m12*Tbt23C13 + m13*Tbt23C12;
|
||||
double sd11 = m22 * Tbt34C34 - m23 * Tbt34C24 + m24 * Tbt34C23; // 3x3 subdeterminants
|
||||
double sd12 = m21 * Tbt34C34 - m23 * Tbt34C14 + m24 * Tbt34C13;
|
||||
double sd13 = m21 * Tbt34C24 - m22 * Tbt34C14 + m24 * Tbt34C12;
|
||||
double sd14 = m21 * Tbt34C23 - m22 * Tbt34C13 + m23 * Tbt34C12;
|
||||
double sd21 = m12 * Tbt34C34 - m13 * Tbt34C24 + m14 * Tbt34C23; // 3x3 subdeterminants
|
||||
double sd22 = m11 * Tbt34C34 - m13 * Tbt34C14 + m14 * Tbt34C13;
|
||||
double sd23 = m11 * Tbt34C24 - m12 * Tbt34C14 + m14 * Tbt34C12;
|
||||
double sd24 = m11 * Tbt34C23 - m12 * Tbt34C13 + m13 * Tbt34C12;
|
||||
double sd31 = m12 * Tbt24C34 - m13 * Tbt24C24 + m14 * Tbt24C23; // 3x3 subdeterminants
|
||||
double sd32 = m11 * Tbt24C34 - m13 * Tbt24C14 + m14 * Tbt24C13;
|
||||
double sd33 = m11 * Tbt24C24 - m12 * Tbt24C14 + m14 * Tbt24C12;
|
||||
double sd34 = m11 * Tbt24C23 - m12 * Tbt24C13 + m13 * Tbt24C12;
|
||||
double sd41 = m12 * Tbt23C34 - m13 * Tbt23C24 + m14 * Tbt23C23; // 3x3 subdeterminants
|
||||
double sd42 = m11 * Tbt23C34 - m13 * Tbt23C14 + m14 * Tbt23C13;
|
||||
double sd43 = m11 * Tbt23C24 - m12 * Tbt23C14 + m14 * Tbt23C12;
|
||||
double sd44 = m11 * Tbt23C23 - m12 * Tbt23C13 + m13 * Tbt23C12;
|
||||
|
||||
double detInv = 1.0/(m11*sd11 - m12*sd12 + m13*sd13 - m14*sd14);
|
||||
double detInv = 1.0 / (m11 * sd11 - m12 * sd12 + m13 * sd13 - m14 * sd14);
|
||||
|
||||
m11 = sd11*detInv;
|
||||
m12 = -sd21*detInv;
|
||||
m13 = sd31*detInv;
|
||||
m14 = -sd41*detInv;
|
||||
m21 = -sd12*detInv;
|
||||
m22 = sd22*detInv;
|
||||
m23 = -sd32*detInv;
|
||||
m24 = sd42*detInv;
|
||||
m31 = sd13*detInv;
|
||||
m32 = -sd23*detInv;
|
||||
m33 = sd33*detInv;
|
||||
m34 = -sd43*detInv;
|
||||
m41 = -sd14*detInv;
|
||||
m42 = sd24*detInv;
|
||||
m43 = -sd34*detInv;
|
||||
m44 = sd44*detInv;
|
||||
m11 = sd11 * detInv;
|
||||
m12 = -sd21 * detInv;
|
||||
m13 = sd31 * detInv;
|
||||
m14 = -sd41 * detInv;
|
||||
m21 = -sd12 * detInv;
|
||||
m22 = sd22 * detInv;
|
||||
m23 = -sd32 * detInv;
|
||||
m24 = sd42 * detInv;
|
||||
m31 = sd13 * detInv;
|
||||
m32 = -sd23 * detInv;
|
||||
m33 = sd33 * detInv;
|
||||
m34 = -sd43 * detInv;
|
||||
m41 = -sd14 * detInv;
|
||||
m42 = sd24 * detInv;
|
||||
m43 = -sd34 * detInv;
|
||||
m44 = sd44 * detInv;
|
||||
|
||||
return ( *this );
|
||||
return (*this);
|
||||
}
|
||||
|
||||
VectorR4 LinearMapR4::Solve(const VectorR4& u) const // Returns solution
|
||||
{
|
||||
VectorR4 LinearMapR4::Solve(const VectorR4& u) const // Returns solution
|
||||
{
|
||||
// Just uses Inverse() for now.
|
||||
return ( Inverse()*u );
|
||||
return (Inverse() * u);
|
||||
}
|
||||
|
||||
// ******************************************************
|
||||
// * RotationMapR4 class - math library functions *
|
||||
// * * * * * * * * * * * * * * * * * * * * * * * * * * **
|
||||
|
||||
|
||||
// ***************************************************************
|
||||
// * 4-space vector and matrix utilities *
|
||||
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
||||
|
||||
// Returns u * v^T
|
||||
LinearMapR4 TimesTranspose( const VectorR4& u, const VectorR4& v)
|
||||
LinearMapR4 TimesTranspose(const VectorR4& u, const VectorR4& v)
|
||||
{
|
||||
LinearMapR4 result;
|
||||
TimesTranspose( u, v, result );
|
||||
TimesTranspose(u, v, result);
|
||||
return result;
|
||||
}
|
||||
|
||||
// The following routines are use to obtain
|
||||
// The following routines are use to obtain
|
||||
// a righthanded orthonormal basis to complement vectors u,v,w.
|
||||
// The vectors u,v,w must be unit and orthonormal.
|
||||
// The value is returned in "rotmat" with the first column(s) of
|
||||
// rotmat equal to u,v,w as appropriate.
|
||||
|
||||
void GetOrtho( const VectorR4& u, RotationMapR4& rotmat )
|
||||
void GetOrtho(const VectorR4& u, RotationMapR4& rotmat)
|
||||
{
|
||||
rotmat.SetColumn1(u);
|
||||
GetOrtho( 1, rotmat );
|
||||
GetOrtho(1, rotmat);
|
||||
}
|
||||
|
||||
void GetOrtho( const VectorR4& u, const VectorR4& v, RotationMapR4& rotmat )
|
||||
void GetOrtho(const VectorR4& u, const VectorR4& v, RotationMapR4& rotmat)
|
||||
{
|
||||
rotmat.SetColumn1(u);
|
||||
rotmat.SetColumn2(v);
|
||||
GetOrtho( 2, rotmat );
|
||||
GetOrtho(2, rotmat);
|
||||
}
|
||||
|
||||
void GetOrtho( const VectorR4& u, const VectorR4& v, const VectorR4& s,
|
||||
RotationMapR4& rotmat )
|
||||
void GetOrtho(const VectorR4& u, const VectorR4& v, const VectorR4& s,
|
||||
RotationMapR4& rotmat)
|
||||
{
|
||||
rotmat.SetColumn1(u);
|
||||
rotmat.SetColumn2(v);
|
||||
rotmat.SetColumn3(s);
|
||||
GetOrtho( 3, rotmat );
|
||||
GetOrtho(3, rotmat);
|
||||
}
|
||||
|
||||
// This final version of GetOrtho is mainly for internal use.
|
||||
// It uses a Gram-Schmidt procedure to extend a partial orthonormal
|
||||
// basis to a complete orthonormal basis.
|
||||
// j = number of columns of rotmat that have already been set.
|
||||
void GetOrtho( int j, RotationMapR4& rotmat)
|
||||
void GetOrtho(int j, RotationMapR4& rotmat)
|
||||
{
|
||||
if ( j==0 ) {
|
||||
if (j == 0)
|
||||
{
|
||||
rotmat.SetIdentity();
|
||||
return;
|
||||
}
|
||||
if ( j==1 ) {
|
||||
rotmat.SetColumn2( -rotmat.m21, rotmat.m11, -rotmat.m41, rotmat.m31 );
|
||||
if (j == 1)
|
||||
{
|
||||
rotmat.SetColumn2(-rotmat.m21, rotmat.m11, -rotmat.m41, rotmat.m31);
|
||||
j = 2;
|
||||
}
|
||||
|
||||
assert ( rotmat.Column1().Norm()<1.0001 && 0.9999<rotmat.Column1().Norm()
|
||||
&& rotmat.Column1().Norm()<1.0001 && 0.9999<rotmat.Column1().Norm()
|
||||
&& (rotmat.Column1()^rotmat.Column2()) < 0.001
|
||||
&& (rotmat.Column1()^rotmat.Column2()) > -0.001 );
|
||||
assert(rotmat.Column1().Norm() < 1.0001 && 0.9999 < rotmat.Column1().Norm() && rotmat.Column1().Norm() < 1.0001 && 0.9999 < rotmat.Column1().Norm() && (rotmat.Column1() ^ rotmat.Column2()) < 0.001 && (rotmat.Column1() ^ rotmat.Column2()) > -0.001);
|
||||
|
||||
// 2x2 subdeterminants in first 2 columns
|
||||
|
||||
double d12 = rotmat.m11*rotmat.m22-rotmat.m12*rotmat.m21;
|
||||
double d13 = rotmat.m11*rotmat.m32-rotmat.m12*rotmat.m31;
|
||||
double d14 = rotmat.m11*rotmat.m42-rotmat.m12*rotmat.m41;
|
||||
double d23 = rotmat.m21*rotmat.m32-rotmat.m22*rotmat.m31;
|
||||
double d24 = rotmat.m21*rotmat.m42-rotmat.m22*rotmat.m41;
|
||||
double d34 = rotmat.m31*rotmat.m42-rotmat.m32*rotmat.m41;
|
||||
double d12 = rotmat.m11 * rotmat.m22 - rotmat.m12 * rotmat.m21;
|
||||
double d13 = rotmat.m11 * rotmat.m32 - rotmat.m12 * rotmat.m31;
|
||||
double d14 = rotmat.m11 * rotmat.m42 - rotmat.m12 * rotmat.m41;
|
||||
double d23 = rotmat.m21 * rotmat.m32 - rotmat.m22 * rotmat.m31;
|
||||
double d24 = rotmat.m21 * rotmat.m42 - rotmat.m22 * rotmat.m41;
|
||||
double d34 = rotmat.m31 * rotmat.m42 - rotmat.m32 * rotmat.m41;
|
||||
VectorR4 vec3;
|
||||
|
||||
if ( j==2 ) {
|
||||
if ( d12>0.4 || d12<-0.4 || d13>0.4 || d13<-0.4
|
||||
|| d23>0.4 || d23<-0.4 ) {
|
||||
vec3.Set( d23, -d13, d12, 0.0);
|
||||
if (j == 2)
|
||||
{
|
||||
if (d12 > 0.4 || d12 < -0.4 || d13 > 0.4 || d13 < -0.4 || d23 > 0.4 || d23 < -0.4)
|
||||
{
|
||||
vec3.Set(d23, -d13, d12, 0.0);
|
||||
}
|
||||
else if ( d24>0.4 || d24<-0.4 || d14>0.4 || d14<-0.4 ) {
|
||||
vec3.Set( d24, -d14, 0.0, d12 );
|
||||
else if (d24 > 0.4 || d24 < -0.4 || d14 > 0.4 || d14 < -0.4)
|
||||
{
|
||||
vec3.Set(d24, -d14, 0.0, d12);
|
||||
}
|
||||
else {
|
||||
vec3.Set( d34, 0.0, -d14, d13 );
|
||||
else
|
||||
{
|
||||
vec3.Set(d34, 0.0, -d14, d13);
|
||||
}
|
||||
vec3.Normalize();
|
||||
rotmat.SetColumn3(vec3);
|
||||
@@ -378,90 +380,88 @@ void GetOrtho( int j, RotationMapR4& rotmat)
|
||||
|
||||
// Do the final column
|
||||
|
||||
rotmat.SetColumn4 (
|
||||
-rotmat.m23*d34 + rotmat.m33*d24 - rotmat.m43*d23,
|
||||
rotmat.m13*d34 - rotmat.m33*d14 + rotmat.m43*d13,
|
||||
-rotmat.m13*d24 + rotmat.m23*d14 - rotmat.m43*d12,
|
||||
rotmat.m13*d23 - rotmat.m23*d13 + rotmat.m33*d12 );
|
||||
|
||||
assert ( 0.99 < (((LinearMapR4)rotmat)).Determinant()
|
||||
&& (((LinearMapR4)rotmat)).Determinant() < 1.01 );
|
||||
rotmat.SetColumn4(
|
||||
-rotmat.m23 * d34 + rotmat.m33 * d24 - rotmat.m43 * d23,
|
||||
rotmat.m13 * d34 - rotmat.m33 * d14 + rotmat.m43 * d13,
|
||||
-rotmat.m13 * d24 + rotmat.m23 * d14 - rotmat.m43 * d12,
|
||||
rotmat.m13 * d23 - rotmat.m23 * d13 + rotmat.m33 * d12);
|
||||
|
||||
assert(0.99 < (((LinearMapR4)rotmat)).Determinant() && (((LinearMapR4)rotmat)).Determinant() < 1.01);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// *********************************************************************
|
||||
// Rotation routines *
|
||||
// *********************************************************************
|
||||
|
||||
// Rotate unit vector x in the direction of "dir": length of dir is rotation angle.
|
||||
// x must be a unit vector. dir must be perpindicular to x.
|
||||
VectorR4& VectorR4::RotateUnitInDirection ( const VectorR4& dir)
|
||||
{
|
||||
assert ( this->Norm()<1.0001 && this->Norm()>0.9999 &&
|
||||
(dir^(*this))<0.0001 && (dir^(*this))>-0.0001 );
|
||||
VectorR4& VectorR4::RotateUnitInDirection(const VectorR4& dir)
|
||||
{
|
||||
assert(this->Norm() < 1.0001 && this->Norm() > 0.9999 &&
|
||||
(dir ^ (*this)) < 0.0001 && (dir ^ (*this)) > -0.0001);
|
||||
|
||||
double theta = dir.NormSq();
|
||||
if ( theta==0.0 ) {
|
||||
if (theta == 0.0)
|
||||
{
|
||||
return *this;
|
||||
}
|
||||
else {
|
||||
else
|
||||
{
|
||||
theta = sqrt(theta);
|
||||
double costheta = cos(theta);
|
||||
double sintheta = sin(theta);
|
||||
VectorR4 dirUnit = dir/theta;
|
||||
*this = costheta*(*this) + sintheta*dirUnit;
|
||||
VectorR4 dirUnit = dir / theta;
|
||||
*this = costheta * (*this) + sintheta * dirUnit;
|
||||
// this->NormalizeFast();
|
||||
return ( *this );
|
||||
return (*this);
|
||||
}
|
||||
}
|
||||
|
||||
// RotateToMap returns a RotationMapR4 that rotates fromVec to toVec,
|
||||
// leaving the orthogonal subspace fixed.
|
||||
// fromVec and toVec should be unit vectors
|
||||
RotationMapR4 RotateToMap( const VectorR4& fromVec, const VectorR4& toVec)
|
||||
RotationMapR4 RotateToMap(const VectorR4& fromVec, const VectorR4& toVec)
|
||||
{
|
||||
LinearMapR4 result;
|
||||
LinearMapR4 result;
|
||||
result.SetIdentity();
|
||||
LinearMapR4 temp;
|
||||
VectorR4 vPerp = ProjectPerpUnitDiff( toVec, fromVec );
|
||||
double sintheta = vPerp.Norm(); // theta = angle between toVec and fromVec
|
||||
VectorR4 vProj = toVec-vPerp;
|
||||
double costheta = vProj^fromVec;
|
||||
if ( sintheta == 0.0 ) {
|
||||
VectorR4 vPerp = ProjectPerpUnitDiff(toVec, fromVec);
|
||||
double sintheta = vPerp.Norm(); // theta = angle between toVec and fromVec
|
||||
VectorR4 vProj = toVec - vPerp;
|
||||
double costheta = vProj ^ fromVec;
|
||||
if (sintheta == 0.0)
|
||||
{
|
||||
// The vectors either coincide (return identity) or directly oppose
|
||||
if ( costheta < 0.0 ) {
|
||||
result = -result; // Vectors directly oppose: return -identity.
|
||||
if (costheta < 0.0)
|
||||
{
|
||||
result = -result; // Vectors directly oppose: return -identity.
|
||||
}
|
||||
}
|
||||
else {
|
||||
vPerp /= sintheta; // Normalize
|
||||
VectorProjectMap ( fromVec, temp ); // project in fromVec direction
|
||||
temp *= (costheta-1.0);
|
||||
else
|
||||
{
|
||||
vPerp /= sintheta; // Normalize
|
||||
VectorProjectMap(fromVec, temp); // project in fromVec direction
|
||||
temp *= (costheta - 1.0);
|
||||
result += temp;
|
||||
VectorProjectMap ( vPerp, temp ); // Project in vPerp direction
|
||||
temp *= (costheta-1.0);
|
||||
VectorProjectMap(vPerp, temp); // Project in vPerp direction
|
||||
temp *= (costheta - 1.0);
|
||||
result += temp;
|
||||
TimesTranspose ( vPerp, fromVec, temp ); // temp = (vPerp)*(fromVec^T)
|
||||
TimesTranspose(vPerp, fromVec, temp); // temp = (vPerp)*(fromVec^T)
|
||||
temp *= sintheta;
|
||||
result += temp;
|
||||
temp.MakeTranspose();
|
||||
result -= temp; // (-sintheta)*(fromVec)*(vPerp^T)
|
||||
result -= temp; // (-sintheta)*(fromVec)*(vPerp^T)
|
||||
}
|
||||
RotationMapR4 rotationResult;
|
||||
rotationResult.Set(result); // Make explicitly a RotationMapR4
|
||||
rotationResult.Set(result); // Make explicitly a RotationMapR4
|
||||
return rotationResult;
|
||||
}
|
||||
|
||||
|
||||
// ***************************************************************
|
||||
// Stream Output Routines *
|
||||
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
||||
|
||||
ostream& operator<< ( ostream& os, const VectorR4& u )
|
||||
ostream& operator<<(ostream& os, const VectorR4& u)
|
||||
{
|
||||
return (os << "<" << u.x << "," << u.y << "," << u.z << "," << u.w << ">");
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user