void
avtMatrix::MakeView(const avtVector &from,
const avtVector &at,
const avtVector &world_up)
{
avtVector up, right, view_dir;
view_dir = (at - from).normalized();
right = (world_up % view_dir).normalized();
up = (view_dir % right).normalized();
MakeIdentity();
m[0][0] = right.x;
m[0][1] = right.y;
m[0][2] = right.z;
m[1][0] = up.x;
m[1][1] = up.y;
m[1][2] = up.z;
m[2][0] = view_dir.x;
m[2][1] = view_dir.y;
m[2][2] = view_dir.z;
m[0][3] = -(right*from);
m[1][3] = -(up*from);
m[2][3] = -(view_dir*from);
}
void Matrix::Transform(Vec3d &v)
{
MakeIdentity();
d[0][3] = v.x;
d[1][3] = v.y;
d[2][3] = v.z;
}
void
avtMatrix::Inverse()
{
avtMatrix n, y;
int i, j, indx[4];
double d, col[4];
n=*this;
if (ludcmp(&n, indx, &d)) {
MakeIdentity();
return;
}
for (j=0; j<4; j++) {
for (i=0; i<4; i++) {
col[i] = 0.0f;
}
col[j] = 1.0f;
lubksb(&n, indx, col);
for (i=0; i<4; i++) {
y.m[i][j] = col[i];
}
}
*this = y;
return;
}
void
avtMatrix::MakeRBT(const avtVector &from,
const avtVector &at,
const avtVector &world_up)
{
avtVector up, right, view_dir;
view_dir = (at - from).normalized();
right = (world_up % view_dir).normalized();
up = (view_dir % right).normalized();
MakeIdentity();
m[0][0] = right.x;
m[0][1] = right.y;
m[0][2] = right.z;
m[1][0] = up.x;
m[1][1] = up.y;
m[1][2] = up.z;
m[2][0] = view_dir.x;
m[2][1] = view_dir.y;
m[2][2] = view_dir.z;
//
// The matrix so far will put us in the local coordinate system...
// We want the inverse of that.
//
Inverse();
// Don't forget the translation
m[0][3] = from.x;
m[1][3] = from.y;
m[2][3] = from.z;
}
inline void
Identity( DistMatrix<T,U,V>& I, Int m, Int n )
{
DEBUG_ONLY(CallStackEntry cse("Identity"))
I.Resize( m, n );
MakeIdentity( I );
}
inline DistMatrix<T,U,V>
Identity( const Grid& g, Int m, Int n )
{
DistMatrix<T,U,V> I( m, n, g );
MakeIdentity( I );
return I;
}
inline Matrix<T>
Identity( Int m, Int n )
{
Matrix<T> I( m, n );
MakeIdentity( I );
return I;
}
void
avtMatrix::MakeFrameToFrameConversion(
const avtVector &u1, const avtVector &v1, const avtVector &w1, const avtVector &o1,
const avtVector &u2, const avtVector &v2, const avtVector &w2, const avtVector &o2)
{
avtVector t = (o1-o2);
MakeIdentity();
m[0][0] = u1*u2;
m[1][0] = u1*v2;
m[2][0] = u1*w2;
m[0][1] = v1*u2;
m[1][1] = v1*v2;
m[2][1] = v1*w2;
m[0][2] = w1*u2;
m[1][2] = w1*v2;
m[2][2] = w1*w2;
m[0][3] = t*u2;
m[1][3] = t*v2;
m[2][3] = t*w2;
}
Quat& Quat::RotationFromTo(const Vec3& from, const Vec3& to)
{
// Based on Stan Melax's article in Game Programming Gems
// Copy, since cannot modify local
Vec3 v0 = from;
Vec3 v1 = to;
v0.normalize();
v1.normalize();
const float d = v0.dot(v1);
if (d >= 1.0f) // If dot == 1, vectors are the same
{
return MakeIdentity();
}
else if (d <= -1.0f) // exactly opposite
{
Vec3 axis(1.0f, 0.f, 0.f);
axis = axis.cross(v0);
if (axis.getLength()==0)
{
axis.set(0.f,1.f,0.f);
axis = axis.cross(v0);
}
// same as fromAngleAxis(core::PI, axis).normalize();
return Set(axis.x, axis.y, axis.z, 0).Normalize();
}
const float s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
const float invs = 1.f / s;
const Vec3 c = v0.cross(v1)*invs;
return Set(c.x, c.y, c.z, s * 0.5f).Normalize();
}
void
avtMatrix::MakeScale(double s)
{
MakeIdentity();
m[0][0] = s;
m[1][1] = s;
m[2][2] = s;
}
void
avtMatrix::MakeTranslate(double x,double y,double z)
{
MakeIdentity();
m[0][3] = x;
m[1][3] = y;
m[2][3] = z;
}
void
avtMatrix::MakeScale(double x,double y,double z)
{
MakeIdentity();
m[0][0] = x;
m[1][1] = y;
m[2][2] = z;
}
void
avtMatrix::MakeTranslate(const avtVector &t)
{
MakeIdentity();
m[0][3] = t.x;
m[1][3] = t.y;
m[2][3] = t.z;
}
//----------------------------------------------------------------------------
//
// Fixed Matrix 2 Implementation
//
//----------------------------------------------------------------------------
Matrix2x::Matrix2x (bool bZero)
{
if (bZero)
{
MakeZero();
}
else
{
MakeIdentity();
}
}
// MakeRotationX()
//------------------------------------------------------------------------------
void Mat44::MakeRotationX( float angleRadians )
{
float cosa = Cos(angleRadians);
float sina = Sin(angleRadians);
MakeIdentity();
col1.y = cosa;
col2.z = cosa;
col1.z = -sina;
col2.y = sina;
}
// MakeRotationY()
//------------------------------------------------------------------------------
void Mat44::MakeRotationY( float angleRadians )
{
float cosa = Cos(angleRadians);
float sina = Sin(angleRadians);
MakeIdentity();
col0.x = cosa;
col2.z = cosa;
col0.z = sina;
col2.x = -sina;
}
// MakeRotationZ()
//------------------------------------------------------------------------------
void Mat44::MakeRotationZ( float angleRadians )
{
float cosa = Cos(angleRadians);
float sina = Sin(angleRadians);
MakeIdentity();
col0.x = cosa;
col1.y = cosa;
col0.y = -sina;
col1.x = sina;
}
void
avtMatrix::MakeOrthographicProjection(double size,
double near_plane,
double far_plane,
double aspect)
{
double d;
d = far_plane - near_plane;
MakeIdentity();
m[0][0] = 2./(size*aspect);
m[1][1] = 2./size;
m[2][2] = 1./d;
m[2][3] = -near_plane/d;
m[3][3] = 1;
}
void MakeExtendedKahan
( ElementalMatrix<F>& A, Base<F> phi, Base<F> mu )
{
EL_DEBUG_CSE
typedef Base<F> Real;
if( A.Height() != A.Width() )
LogicError("Extended Kahan matrices must be square");
const Int n = A.Height();
if( n % 3 != 0 )
LogicError("Dimension must be an integer multiple of 3");
const Int l = n / 3;
if( !l || (l & (l-1)) )
LogicError("n/3 is not a power of two");
Int k=0;
while( Int(1u<<k) < l )
++k;
if( phi <= Real(0) || phi >= Real(1) )
LogicError("phi must be in (0,1)");
if( mu <= Real(0) || mu >= Real(1) )
LogicError("mu must be in (0,1)");
// Start by setting A to the identity, and then modify the necessary
// l x l blocks of its 3 x 3 partitioning.
MakeIdentity( A );
unique_ptr<ElementalMatrix<F>> ABlock( A.Construct(A.Grid(),A.Root()) );
View( *ABlock, A, IR(2*l,3*l), IR(2*l,3*l) );
*ABlock *= mu;
View( *ABlock, A, IR(0,l), IR(l,2*l) );
Walsh( *ABlock, k );
*ABlock *= -phi;
View( *ABlock, A, IR(l,2*l), IR(2*l,3*l) );
Walsh( *ABlock, k );
*ABlock *= phi;
// Now scale A by S
const Real zeta = Sqrt(Real(1)-phi*phi);
auto& ALoc = A.Matrix();
for( Int iLoc=0; iLoc<A.LocalHeight(); ++iLoc )
{
const Int i = A.GlobalRow(iLoc);
const Real gamma = Pow(zeta,Real(i));
for( Int jLoc=0; jLoc<A.LocalWidth(); ++jLoc )
ALoc(iLoc,jLoc) *= gamma;
}
}
moGLMatrixf&
moGLMatrixf::MakeOrthographic( float left, float right, float bottom, float top, float znear, float zfar ) {
float r_l = right - left;
float t_b = top - bottom;
float f_n = zfar - znear;
float tx = -(right + left)/r_l;
float ty = -(top + bottom)/t_b;
float tz = -(zfar + znear)/f_n;
MakeIdentity();
moGLMatrixf& Me( *this );
moGLMatrixf Result = moMatrix4f::IDENTITY;
Result.SetRow( 0, moVector4f( 2.0 / r_l, 0.0, 0.0, tx ) );
Result.SetRow( 1, moVector4f( 0.0, 2.0 / t_b, 0.0, ty ) );
Result.SetRow( 2, moVector4f( 0.0, 0.0, -2.0 / f_n, tz ) );
Me = Me * (Result.Transpose());
return (*this);
}
void CMatrix::MakeLookAt (const CVector3 &vEye,
const CVector3 &vCenter, const CVector3 &vUp)
{
//
// Compute the unit vector direction from the eye to the center
//
CVector3 z (vEye - vCenter);
z .Normalize ();
//
// Using the eye and the up, compute the normal from those two
// vectors.
//
CVector3 x (vUp .Cross (z));
//
// Compute y which is a perpindicular version of vUp (y == vUp basically)
//
CVector3 y (z .Cross (x));
//
// Normalize the two vectors
//
x .Normalize ();
y .Normalize ();
//
// Initialize the matrix
//
MakeIdentity ();
SetCol (0, x);
SetCol (1, y);
SetCol (2, z);
//
// Add the translation
//
Translate (-vEye);
}
void MakeExtendedKahan
( Matrix<F>& A, Base<F> phi, Base<F> mu )
{
EL_DEBUG_CSE
typedef Base<F> Real;
if( A.Height() != A.Width() )
LogicError("Extended Kahan matrices must be square");
const Int n = A.Height();
if( n % 3 != 0 )
LogicError("Dimension must be an integer multiple of 3");
const Int l = n / 3;
if( !l || (l & (l-1)) )
LogicError("n/3 is not a power of two");
Int k=0;
while( Int(1u<<k) < l )
++k;
if( phi <= Real(0) || phi >= Real(1) )
LogicError("phi must be in (0,1)");
if( mu <= Real(0) || mu >= Real(1) )
LogicError("mu must be in (0,1)");
// Start by setting A to the identity, and then modify the necessary
// l x l blocks of its 3 x 3 partitioning.
MakeIdentity( A );
auto ABlock = A( IR(2*l,3*l), IR(2*l,3*l) );
ABlock *= mu;
ABlock = A( IR(0,l), IR(l,2*l) );
Walsh( ABlock, k );
ABlock *= -phi;
ABlock = A( IR(l,2*l), IR(2*l,3*l) );
Walsh( ABlock, k );
ABlock *= phi;
// Now scale A by S
const Real zeta = Sqrt(Real(1)-phi*phi);
for( Int i=0; i<n; ++i )
{
const Real gamma = Pow(zeta,Real(i));
for( Int j=0; j<n; ++j )
A(i,j) *= gamma;
}
}
void
avtMatrix::MakeRotation(const avtVector &from,
const avtVector &at,
const avtVector &world_up)
{
avtVector new_z = (from - at).normalized();
avtVector new_x = (world_up % new_z).normalized();
avtVector new_y = (new_z % new_x).normalized();
MakeIdentity();
m[0][0] = new_x.x;
m[0][1] = new_y.x;
m[0][2] = new_z.x;
m[1][0] = new_x.y;
m[1][1] = new_y.y;
m[1][2] = new_z.y;
m[2][0] = new_x.z;
m[2][1] = new_y.z;
m[2][2] = new_z.z;
}
moGLMatrixf&
moGLMatrixf::MakeFrustrum( float left, float right, float bottom, float top, float znear, float zfar ) {
float r_l = right - left;
float t_b = top - bottom;
float f_n = zfar - znear;
float A = (right + left)/r_l;
float B = (top + bottom)/t_b;
float C = -(zfar + znear)/f_n;
float D = -2*(zfar*znear)/f_n;
MakeIdentity();
moGLMatrixf& Me( *this );
moGLMatrixf Result = moMatrix4f::IDENTITY;
Result.SetRow( 0, moVector4f( 2.0 * znear / r_l, 0.0, A, 0.0 ) );
Result.SetRow( 1, moVector4f( 0.0, 2.0 * znear / t_b, B, 0.0 ) );
Result.SetRow( 2, moVector4f( 0.0, 0.0, C, D ) );
Result.SetRow( 3, moVector4f( 0.0, 0.0, -1.0, 0.0 ) );
Me = Me * (Result.Transpose());
return (*this);
}
void CVisMatrix::MakeRotation( const CVisFixpoint & fpAxis_x, const CVisFixpoint & fpAxis_y, const CVisFixpoint & fpAxis_z, const CVisFixpoint & fpAngle )
{
float cos_al = (float)cos( fpAngle.GetFloatValue() );
float sin_al = (float)sin( fpAngle.GetFloatValue() );
float one_cos_al = 1-cos_al;
float a_x, a_y, a_z;
a_x = fpAxis_x.GetFloatValue();
a_y = fpAxis_y.GetFloatValue();
a_z = fpAxis_z.GetFloatValue();
// normalize axis.
float len = (float)sqrt(a_x*a_x + a_y*a_y + a_z*a_z);
if ( len < 0.000001 )
return;
if ( len != 1.0 )
{
a_x /= len;
a_y /= len;
a_z /= len;
}
// First, generate the identity matrix
MakeIdentity();
// Now generate the three columns of the rotation matrix
(*this)( 1,1 ) = one_cos_al*a_x*a_x + cos_al;
(*this)( 2,1 ) = one_cos_al*a_x*a_y + sin_al*a_z;
(*this)( 3,1 ) = one_cos_al*a_x*a_z - sin_al*a_y;
(*this)( 1,2 ) = one_cos_al*a_x*a_y - sin_al*a_z;
(*this)( 2,2 ) = one_cos_al*a_y*a_y + cos_al;
(*this)( 3,2 ) = one_cos_al*a_y*a_z + sin_al*a_x;
(*this)( 1,3 ) = one_cos_al*a_x*a_z + sin_al*a_y;
(*this)( 2,3 ) = one_cos_al*a_y*a_z - sin_al*a_x;
(*this)( 3,3 ) = one_cos_al*a_z*a_z + cos_al;
// Don't have to set column 4...
}
void
avtMatrix::MakeFrameToCartesianConversion(const avtVector &u, const avtVector &v,
const avtVector &w, const avtVector &o)
{
MakeIdentity();
m[0][0] = u.x;
m[1][0] = u.y;
m[2][0] = u.z;
m[0][1] = v.x;
m[1][1] = v.y;
m[2][1] = v.z;
m[0][2] = w.x;
m[1][2] = w.y;
m[2][2] = w.z;
m[0][3] = o.x;
m[1][3] = o.y;
m[2][3] = o.z;
}
int InverseFreeSign( ElementalMatrix<F>& XPre, Int maxIts=100, Base<F> tau=0 )
{
DEBUG_CSE
DistMatrixReadWriteProxy<F,F,MC,MR> XProx( XPre );
auto& X = XProx.Get();
typedef Base<F> Real;
const Grid& g = X.Grid();
const Int n = X.Width();
if( X.Height() != 2*n )
LogicError("X must be 2n x n");
// Compute the tolerance if it is unset
if( tau == Real(0) )
tau = n*limits::Epsilon<Real>();
// Expose A and B in the original and temporary
DistMatrix<F> XAlt( 2*n, n, g );
auto B = X( IR(0,n ), ALL );
auto A = X( IR(n,2*n), ALL );
auto BAlt = XAlt( IR(0,n ), ALL );
auto AAlt = XAlt( IR(n,2*n), ALL );
// Flip the sign of A
A *= -1;
// Set up the space for explicitly computing the left half of Q
DistMatrix<F,MD,STAR> t(g);
DistMatrix<Base<F>,MD,STAR> d(g);
DistMatrix<F> Q( 2*n, n, g );
auto Q12 = Q( IR(0,n ), ALL );
auto Q22 = Q( IR(n,2*n), ALL );
// Run the iterative algorithm
Int numIts=0;
DistMatrix<F> R(g), RLast(g);
while( numIts < maxIts )
{
XAlt = X;
QR( XAlt, t, d );
// Form the left half of Q
Zero( Q12 );
MakeIdentity( Q22 );
qr::ApplyQ( LEFT, NORMAL, XAlt, t, d, Q );
// Save a copy of R
R = BAlt;
MakeTrapezoidal( UPPER, R );
// Form the new iterate
Gemm( ADJOINT, NORMAL, F(1), Q12, A, F(0), AAlt );
Gemm( ADJOINT, NORMAL, F(1), Q22, B, F(0), BAlt );
X = XAlt;
// Use the difference in the iterates to test for convergence
++numIts;
if( numIts > 1 )
{
const Real oneRLast = OneNorm(RLast);
AxpyTrapezoid( UPPER, F(-1), R, RLast );
const Real oneRDiff = OneNorm(RLast);
if( oneRDiff <= tau*oneRLast )
break;
}
RLast = R;
}
// Revert the sign of A and return
A *= -1;
return numIts;
}
void Matrix4::MakeTranslationMatrix(float aX, float aY, float aZ)
{
MakeIdentity();
SetTranslationData(Vector3(aX,aY,aZ));
}
void Matrix4::MakeTranslationMatrix(const Vector3 &aVec)
{
MakeIdentity();
SetTranslationData(aVec);
}
void Identity( AbstractBlockDistMatrix<T>& I, Int m, Int n )
{
DEBUG_ONLY(CallStackEntry cse("Identity"))
I.Resize( m, n );
MakeIdentity( I );
}
