double ECHARM_cell::FindVectorSquaredReciprocal(int vIndex[3])
{
double double_result = 0.0;
double double_temp[6];
int i;
if(IsOrthogonal())
{
for(i=0;i<3;i++) double_result += fSquare(vIndex[i] / fSize->GetComponent(i));
}
else
{
double_temp[0] = fSize->GetComponent(1) * fSize->GetComponent(2) * sin(fAngle->GetComponent(1)) / FindVolume();
double_temp[1] = fSize->GetComponent(0) * fSize->GetComponent(2) * sin(fAngle->GetComponent(0)) / FindVolume();
double_temp[2] = fSize->GetComponent(0) * fSize->GetComponent(1) * sin(fAngle->GetComponent(2)) / FindVolume();
double_temp[3] = fSize->GetComponent(0) * fSize->GetComponent(1) * fSize->GetComponent(2) * fSize->GetComponent(2) * (cos(fAngle->GetComponent(0)) * cos(fAngle->GetComponent(1)) - cos(fAngle->GetComponent(2))) / FindVolume();
double_temp[4] = fSize->GetComponent(0) * fSize->GetComponent(1) * fSize->GetComponent(1) * fSize->GetComponent(2) * (cos(fAngle->GetComponent(2)) * cos(fAngle->GetComponent(0)) - cos(fAngle->GetComponent(1))) / FindVolume();
double_temp[5] = fSize->GetComponent(0) * fSize->GetComponent(0) * fSize->GetComponent(1) * fSize->GetComponent(2) * (cos(fAngle->GetComponent(1)) * cos(fAngle->GetComponent(2)) - cos(fAngle->GetComponent(0))) / FindVolume();
for(i=0;i<3;i++) double_temp[i] = double_temp[i] * double_temp[i] * vIndex[i] * vIndex[i];
double_temp[3] *= (2 * vIndex[0] * vIndex[1]);
double_temp[4] *= (2 * vIndex[2] * vIndex[0]);
double_temp[5] *= (2 * vIndex[2] * vIndex[2]);
for(i=0;i<6;i++) double_result += double_temp[i];
}
double_result *= (4 * M_PI * M_PI);
return double_result;
}
//-----------------------------------------------------------------------------
Matrix22 Matrix22::AsInverseOrthogonal()const
{
DIA_ASSERT(IsOrthogonal(), "Must be orthogonal to use this");
Matrix22 m(mElement[0], mElement[2], mElement[1], mElement[3]);
return m;
}
bool ECHARM_cell::IsCubic()
{
if(IsOrthogonal())
if(fSize->GetComponent(0) == fSize->GetComponent(1))
if(fSize->GetComponent(1) == fSize->GetComponent(2))
return 1;
return 0;
}
double ECHARM_cell::FindVolume()
{
double double_temp = fSize->GetComponent(0)*fSize->GetComponent(1)*fSize->GetComponent(2);
if(!IsOrthogonal())
{
double_temp = fSize->GetComponent(0)*fSize->GetComponent(1)*fSize->GetComponent(2)*cos(fAngle->GetComponent(0))*sin(fAngle->GetComponent(2));
}
return double_temp;
}
//-----------------------------------------------------------------------------
void Matrix22::GetRotationCounterClockwise(Angle& result)const
{
DIA_ASSERT(IsOrthogonal(), "Must be orthogonal");
DIA_ASSERT(IsAxisNormal(), "Axis are not normal");
Dia::Maths::Vector2D v1(1.0f, 0.0f);
Dia::Maths::Vector2D v2(mElement[0], mElement[1]);
v2.GetCounterClockwiseAngleBetween(v1, result);
}
//-----------------------------------------------------------------------------
Matrix22& Matrix22::LookAtRotation(const Vector2D& lookFrom, const Vector2D& lookAt)
{
Vector2D xAxis = (lookAt - lookFrom).AsNormal();
Vector2D yAxis = xAxis.AsRotated90DegreeCounterClockwise();
this->Set(xAxis, yAxis);
DIA_ASSERT(IsOrthogonal(), "Must be orthogonal");
return *this;
}
bool float3x3::InverseOrthogonalUniformScale()
{
assume(IsOrthogonal());
assume(HasUniformScale());
Swap(v[0][1], v[1][0]);
Swap(v[0][2], v[2][0]);
Swap(v[1][2], v[2][1]);
const float scale = sqrtf(1.f / float3(v[0][0], v[0][1], v[0][2]).LengthSq());
v[0][0] *= scale; v[0][1] *= scale; v[0][2] *= scale;
v[1][0] *= scale; v[1][1] *= scale; v[1][2] *= scale;
v[2][0] *= scale; v[2][1] *= scale; v[2][2] *= scale;
return true;
}
bool float3x3::InverseOrthogonal()
{
assume(IsOrthogonal());
Swap(v[0][1], v[1][0]);
Swap(v[0][2], v[2][0]);
Swap(v[1][2], v[2][1]);
float scale1 = sqrtf(1.f / float3(v[0][0], v[0][1], v[0][2]).LengthSq());
float scale2 = sqrtf(1.f / float3(v[1][0], v[1][1], v[1][2]).LengthSq());
float scale3 = sqrtf(1.f / float3(v[2][0], v[2][1], v[2][2]).LengthSq());
v[0][0] *= scale1; v[0][1] *= scale2; v[0][2] *= scale3;
v[1][0] *= scale1; v[1][1] *= scale2; v[1][2] *= scale3;
v[2][0] *= scale1; v[2][1] *= scale2; v[2][2] *= scale3;
return true;
}
//-----------------------------------------------------------------------------
Matrix22 Matrix22::AsRightHanded() const
{
DIA_ASSERT(IsOrthogonal(), "Must be orthogonal");
DIA_ASSERT(IsAxisNormal(), "Axis are not normal");
if (IsRightHanded())
{
return *this;
}
Vector2D xAxis, yAxis;
XAxis(xAxis);
YAxis(yAxis);
Matrix22 m(yAxis, xAxis);
return m;
}
//-----------------------------------------------------------------------------
Matrix22& Matrix22::RightHanded()
{
DIA_ASSERT(IsOrthogonal(), "Must be orthogonal");
DIA_ASSERT(IsAxisNormal(), "Axis are not normal");
if (IsRightHanded())
{
return *this;
}
Vector2D xAxis, yAxis;
XAxis(xAxis);
YAxis(yAxis);
this->Set(yAxis, xAxis);
return *this;
}
//------------------------------------------------------------------------------
// virtual bool IsOrthonormal(Real accuracyRequired) =
// GmatRealConstants::REAL_EPSILON)const
//------------------------------------------------------------------------------
bool Rmatrix::IsOrthonormal (Real accuracyRequired) const
{
bool normal = true; // assume it's normal, try to prove it's not
int i, j;
if (isSizedD == false)
{
throw TableTemplateExceptions::UnsizedTable();
}
// create an array of pointers to column vectors
ArrayTemplate< Rvector* > columnVect(colsD);
// initialize the array
for (i = 0; i < colsD; i++)
{
columnVect[i] = new Rvector(rowsD);
}
// copy from matrix
for (i = 0; i < colsD; i++)
{
for (j = 0; j < rowsD; j++)
{
(*columnVect(i))(j) = elementD[j*colsD + i];
}
}
// see if each magnitude of each columnVect is equal to one
for (i = 0; i < colsD && normal; i++)
{
if (!GmatMathUtil::IsZero(columnVect(i)->GetMagnitude() - 1, accuracyRequired))
normal = false;
}
for (i = 0; i < colsD; i++)
{
delete columnVect[i];
}
return ((bool) (normal && IsOrthogonal(accuracyRequired)));
}
double ECHARM_cell::FindVectorSquaredDirect(int vIndex[3])
{
double double_result = 0.0;
double double_temp[6];
int i;
if(IsOrthogonal())
{
for(i=0;i<3;i++) double_result += (vIndex[i]*vIndex[i]*fSize->GetComponent(i)*fSize->GetComponent(i));
}
else
{
for(i=0;i<3;i++) double_temp[i] = (fSize->GetComponent(i) * fSize->GetComponent(i) * vIndex[i] * vIndex[i]);
double_temp[3] = 2 * vIndex[0] * vIndex[1] * fSize->GetComponent(1) * fSize->GetComponent(2) * cos(fAngle->GetComponent(1)) ;
double_temp[4] = 2 * vIndex[2] * vIndex[0] * fSize->GetComponent(0) * fSize->GetComponent(2) * cos(fAngle->GetComponent(0)) ;
double_temp[5] = 2 * vIndex[2] * vIndex[2] * fSize->GetComponent(0) * fSize->GetComponent(1) * cos(fAngle->GetComponent(2)) ;
for(i=0;i<6;i++) double_result += double_temp[i];
}
return double_result;
}
//------------------------------------------------------------------------------
// bool IsOrthonormal(Real accuracyRequired= Real_Constants::REAL_EPSILON) const
//------------------------------------------------------------------------------
bool Rmatrix66::IsOrthonormal(Real accuracyRequired) const
{
bool normal = true; // assume it's normal, try to prove it's not
// create an array of pointers to column vectors
Rvector6 colVec[6];
// copy from matrix
for (int i = 0; i < colsD; i++)
{
for (int j = 0; j < rowsD; j++)
(colVec[i])(j) = elementD[j*colsD + i];
}
// see if each magnitude of each colVec is equal to one
for (int i = 0; i < colsD && normal; i++)
{
if (!GmatMathUtil::IsZero(colVec[i].GetMagnitude() - 1, accuracyRequired))
normal = false;
}
return ((bool) (normal && IsOrthogonal(accuracyRequired)));
}
void Inverse(const Matrix4 &m, Matrix4 &result)
{
// This entire function desparately needs to be optimised as im repeating many a operation several times.
// However, I've left these as is for clarities sake(whatever hope that has with an operation like inverse).
// If the Matrix4 is Orthogonal then we can just transpose it to make things easier.
if (IsOrthogonal(m))
{
return Transpose(m, result);
}
// If it is not orthogonal then we have lots and lots and lots of calculations to do.
// So we need to calculate the co-factors for each element in the new Matrix4....
// The co-factor for a given point in a new Matrix4 is given by.
//
// Determinat of sub-Matrix4
// /-- --\ /-- --\ /-- --\
// | RES XXX XXX XXX| | RES XXX XXX XXX| | RES XXX XXX XXX|
// | XX (m22) XX XXX| MINUS | X (m22) XXX XXX| ADD | XXX XX (m23) XX|
// | XXX XX (m33) XX| MINUS | X XXX XXX (m34)| ADD | XXX XX (m33) XX|
// | XXX XXX X (m44)| | X XXX (m43) XXX| | XXX XX XX (m44)|
// \-- --/ \-- --/ \-- --/ ... ETC
//
//
// To break it down more, for m11 in the inverse the equation is similar to the determinant.
// We cancel out the rows and columns of the subject.
//
// EXAMPLE OF ROW & COLUMN CANCELLING
// /-- --\ /-- --\ /-- --\
// | m11 XXX XXX XXX| | m11 XXX XXX XXX| | m11 XXX XXX XXX|
// | XXX m22 m23 m24| > > | XXX m22 XXX XXX| > > | XXX m22 XXX XXX|
// | XXX m32 m33 m34| > > | XXX XXX m33 m34| > > | XXX XXX m33 XXX|
// | XXX m42 m43 m44| | XXX XXX m43 m44| | XXX XXX XXX m44|
// \-- --/ \-- --/ \-- --/
//
// m11(-1) = m22*(m33*m44 - m34*m43) + m23*(m34*m42 - m32*m44) + m24*(m32*m43 - m33*m42);
//
// Finally after getting the co-factor we need divide the entire Matrix4 by the determinant of the
// old Matrix4.
// Co-factors of the X Axis vector.
float cof11, cof12, cof13, cof14;
// Co-factors of the Y Axis vector.
float cof21, cof22, cof23, cof24;
// Co-factors of the Z Axis vector.
float cof31, cof32, cof33, cof34;
// Co-factors of the W Axis vector.
float cof41, cof42, cof43, cof44;
// The determinant of the inputed Matrix4.
Matrix4 md(m);
float detInv = 1.0f / md.Determinant();
// And so it starts...
//*********************************************************************************************************
// X AXIS VECTOR
//*********************************************************************************************************
// Cofactor of element m11 aka X component of the x axis vector.
cof11 = m.yY*(m.zZ*m.wW - m.zW*m.wZ) + m.yZ*(m.zW*m.wY - m.zY*m.wW) + m.yW*(m.zY*m.wZ - m.zZ*m.wY);
// Cofactor of element m12 aka Y component of the x axis vector.
cof12 = -(m.yX*(m.zZ*m.wW - m.zW*m.wZ) + m.yZ*(m.zW*m.wX - m.zX*m.wW) + m.yW*(m.zX*m.wZ - m.zZ*m.wX));
// Cofactor of element m13 aka Z component of the x axis vector.
cof13 = m.yX*(m.zY*m.wW - m.zW*m.wY) + m.yY*(m.zW*m.wX - m.zX*m.wW) + m.yW*(m.zX*m.wY - m.zY*m.wX);
// Cofactor of element m14 aka W component of the x axis vector.
cof14 = -(m.yX*(m.zY*m.wZ - m.zZ*m.wY) + m.yY*(m.zZ*m.wX - m.zX*m.wZ) + m.yZ*(m.zX*m.wY - m.zY*m.wX));
//*********************************************************************************************************
// Y AXIS VECTOR
//*********************************************************************************************************
// Cofactor of element m21 aka X component of the y axis vector.
cof21 = -(m.xY*(m.zZ*m.wW - m.zW*m.wZ) + m.xY*(m.zY*m.wW - m.zW*m.wY) + m.xW*(m.zY*m.wZ - m.zZ*m.wY));
// Cofactor of element m22 aka Y component of the y axis vector.
cof22 = m.xX*(m.zZ*m.wW - m.zW*m.wZ) + m.xZ*(m.wX*m.zW - m.wW*m.zX) + m.xW*(m.zX*m.wZ - m.zZ*m.wX);
// Cofactor of element m23 aka Z component of the y axis vector.
cof23 = -(m.xX*(m.zY*m.wW - m.zW*m.wY) + m.xY*(m.wX*m.zW - m.wW*m.zX) + m.xW*(m.zX*m.wY - m.zY*m.wX));
// Cofactor of element m24 aka W component of the y axis vector.
cof24 = m.xX*(m.zY*m.wZ - m.zZ*m.wY) + m.xY*(m.wX*m.zZ - m.wZ*m.zX) + m.xZ*(m.zX*m.wY - m.zY*m.wX);
//*********************************************************************************************************
// Z AXIS VECTOR
//*********************************************************************************************************
// Cofactor of element m31 aka X component of the z axis vector.
cof31 = m.xY*(m.yZ*m.wW - m.yW*m.wZ) + m.xZ*(m.wY*m.yW - m.wW*m.yY) + m.xW*(m.yY*m.wZ - m.yZ*m.wY);
// Cofactor of element m32 aka Y component of the z axis vector.
cof32 = -(m.xX*(m.yZ*m.wW - m.yW*m.wZ) + m.xZ*(m.wX*m.yW - m.wW*m.yX) + m.xW*(m.yX*m.wZ - m.yZ*m.wX));
// Cofactor of element m33 aka Z component of the z axis vector.
cof33 = m.xX*(m.yY*m.wW - m.yW*m.wY) + m.xY*(m.wX*m.yW - m.wW*m.yX) + m.xW*(m.yX*m.wY - m.yY*m.wX);
// Cofactor of element m34 aka W component of the z axis vector.
cof34 = -(m.xX*(m.yY*m.wZ - m.yZ*m.wY) + m.xY*(m.wX*m.yZ - m.wZ*m.yX) + m.xZ*(m.yX*m.wY - m.yY*m.wX));
//*********************************************************************************************************
// W AXIS VECTOR
//*********************************************************************************************************
// Cofactor of element m41 aka X component of the w axis vector.
cof41 = m.xY*(m.yZ*m.zW - m.yW*m.zZ) + m.xZ*(m.zY*m.yW - m.zW*m.yY) + m.xW*(m.yY*m.zZ - m.yZ*m.zY);
// Cofactor of element m42 aka Y component of the w axis vector.
cof42 = -(m.xX*(m.yZ*m.zW - m.yW*m.zZ) + m.xZ*(m.yW*m.zX - m.yX*m.zW) + m.xW*(m.yX*m.zZ - m.yZ*m.zX));
// Cofactor of element m43 aka Z component of the w axis vector.
cof43 = m.xX*(m.yY*m.zW - m.yW*m.zY) + m.xY*(m.zX*m.yW - m.zW*m.yX) + m.xW*(m.yX*m.zY - m.yY*m.zX);
// Cofactor of element m44 aka Z component of the w axis vector.
cof44 = -(m.xX*(m.yY*m.zZ - m.yZ*m.zY) + m.xY*(m.zX*m.yZ - m.zZ*m.yX) + m.xZ*(m.yX*m.zY - m.yY*m.zX));
// Now we have all the co-factors we punch them into our resultant Matrix4 and divide by the determinant.
// Inverse X axis of m.
result.xX = (cof11 * detInv);
result.xY = (cof12 * detInv);
result.xZ = (cof13 * detInv);
result.xW = (cof14 * detInv);
// Inverse Y axis of m.
result.yX = (cof21 * detInv);
result.yY = (cof22 * detInv);
result.yZ = (cof23 * detInv);
result.yW = (cof24 * detInv);
// Inverse Z axis of m.
result.zX = (cof31 * detInv);
result.zY = (cof32 * detInv);
result.zZ = (cof33 * detInv);
result.zW = (cof34 * detInv);
// Inverse W axis of m.
result.wX = (cof41 * detInv);
result.wY = (cof42 * detInv);
result.wZ = (cof43 * detInv);
result.wW = (cof44 * detInv);
}
bool float3x3::IsOrthonormal(float epsilon) const
{
///\todo Epsilon magnitudes don't match.
return IsOrthogonal(epsilon) && Row(0).IsNormalized(epsilon) && Row(1).IsNormalized(epsilon) && Row(2).IsNormalized(epsilon);
}
//-----------------------------------------------------------------------------
Matrix22& Matrix22::InvertOrthogonal()
{
DIA_ASSERT(IsOrthogonal(), "Must be orthogonal to use this");
return Transpose();
}
