inline void
internal::ApplyPackedReflectorsLLVF
( int offset,
const DistMatrix<R,MC,MR>& H,
DistMatrix<R,MC,MR>& A )
{
#ifndef RELEASE
PushCallStack("internal::ApplyPackedReflectorsLLVF");
if( H.Grid() != A.Grid() )
throw std::logic_error("{H,A} must be distributed over the same grid");
if( offset > 0 )
throw std::logic_error("Transforms cannot extend above matrix");
if( offset < -H.Height() )
throw std::logic_error("Transforms cannot extend below matrix");
if( H.Height() != A.Height() )
throw std::logic_error
("Height of transforms must equal height of target matrix");
#endif
const Grid& g = H.Grid();
// Matrix views
DistMatrix<R,MC,MR>
HTL(g), HTR(g), H00(g), H01(g), H02(g), HPan(g), HPanCopy(g),
HBL(g), HBR(g), H10(g), H11(g), H12(g),
H20(g), H21(g), H22(g);
DistMatrix<R,MC,MR>
AT(g), A0(g),
AB(g), A1(g),
A2(g);
DistMatrix<R,VC, STAR> HPan_VC_STAR(g);
DistMatrix<R,MC, STAR> HPan_MC_STAR(g);
DistMatrix<R,STAR,STAR> SInv_STAR_STAR(g);
DistMatrix<R,STAR,MR > Z_STAR_MR(g);
DistMatrix<R,STAR,VR > Z_STAR_VR(g);
LockedPartitionDownDiagonal
( H, HTL, HTR,
HBL, HBR, 0 );
PartitionDown
( A, AT,
AB, 0 );
while( HTL.Height() < H.Height() && HTL.Width() < H.Width() )
{
LockedRepartitionDownDiagonal
( HTL, /**/ HTR, H00, /**/ H01, H02,
/*************/ /******************/
/**/ H10, /**/ H11, H12,
HBL, /**/ HBR, H20, /**/ H21, H22 );
int HPanHeight = H11.Height() + H21.Height();
int HPanWidth = std::min( H11.Width(), std::max(HPanHeight+offset,0) );
HPan.LockedView( H, H00.Height(), H00.Width(), HPanHeight, HPanWidth );
RepartitionDown
( AT, A0,
/**/ /**/
A1,
AB, A2 );
HPan_MC_STAR.AlignWith( AB );
Z_STAR_MR.AlignWith( AB );
Z_STAR_VR.AlignWith( AB );
Z_STAR_MR.ResizeTo( HPanWidth, AB.Width() );
SInv_STAR_STAR.ResizeTo( HPanWidth, HPanWidth );
Zero( SInv_STAR_STAR );
//--------------------------------------------------------------------//
HPanCopy = HPan;
MakeTrapezoidal( LEFT, LOWER, offset, HPanCopy );
SetDiagonalToOne( LEFT, offset, HPanCopy );
HPan_VC_STAR = HPanCopy;
Syrk
( UPPER, TRANSPOSE,
(R)1, HPan_VC_STAR.LockedLocalMatrix(),
(R)0, SInv_STAR_STAR.LocalMatrix() );
SInv_STAR_STAR.SumOverGrid();
HalveMainDiagonal( SInv_STAR_STAR );
HPan_MC_STAR = HPanCopy;
internal::LocalGemm
( TRANSPOSE, NORMAL,
(R)1, HPan_MC_STAR, AB, (R)0, Z_STAR_MR );
Z_STAR_VR.SumScatterFrom( Z_STAR_MR );
internal::LocalTrsm
( LEFT, UPPER, TRANSPOSE, NON_UNIT,
(R)1, SInv_STAR_STAR, Z_STAR_VR );
Z_STAR_MR = Z_STAR_VR;
internal::LocalGemm
( NORMAL, NORMAL, (R)-1, HPan_MC_STAR, Z_STAR_MR, (R)1, AB );
//--------------------------------------------------------------------//
HPan_MC_STAR.FreeAlignments();
Z_STAR_MR.FreeAlignments();
Z_STAR_VR.FreeAlignments();
SlideLockedPartitionDownDiagonal
( HTL, /**/ HTR, H00, H01, /**/ H02,
/**/ H10, H11, /**/ H12,
/*************/ /******************/
HBL, /**/ HBR, H20, H21, /**/ H22 );
SlidePartitionDown
( AT, A0,
A1,
/**/ /**/
AB, A2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
ApplyPackedReflectorsLUVF
( int offset,
const DistMatrix<R>& H,
DistMatrix<R>& A )
{
#ifndef RELEASE
PushCallStack("internal::ApplyPackedReflectorsLUVF");
if( H.Grid() != A.Grid() )
throw std::logic_error("{H,A} must be distributed over the same grid");
if( offset < 0 || offset > H.Height() )
throw std::logic_error("Transforms out of bounds");
if( H.Width() != A.Height() )
throw std::logic_error
("Width of transforms must equal height of target matrix");
#endif
const Grid& g = H.Grid();
DistMatrix<R>
HTL(g), HTR(g), H00(g), H01(g), H02(g), HPan(g),
HBL(g), HBR(g), H10(g), H11(g), H12(g),
H20(g), H21(g), H22(g);
DistMatrix<R>
AT(g), A0(g), ATop(g),
AB(g), A1(g),
A2(g);
DistMatrix<R> HPanCopy(g);
DistMatrix<R,VC, STAR> HPan_VC_STAR(g);
DistMatrix<R,MC, STAR> HPan_MC_STAR(g);
DistMatrix<R,STAR,STAR> SInv_STAR_STAR(g);
DistMatrix<R,STAR,MR > Z_STAR_MR(g);
DistMatrix<R,STAR,VR > Z_STAR_VR(g);
LockedPartitionDownDiagonal
( H, HTL, HTR,
HBL, HBR, 0 );
PartitionDown
( A, AT,
AB, 0 );
while( HTL.Height() < H.Height() && HTL.Width() < H.Width() )
{
LockedRepartitionDownDiagonal
( HTL, /**/ HTR, H00, /**/ H01, H02,
/*************/ /******************/
/**/ H10, /**/ H11, H12,
HBL, /**/ HBR, H20, /**/ H21, H22 );
const int HPanHeight = H01.Height() + H11.Height();
const int HPanOffset =
std::min( H11.Width(), std::max(offset-H00.Width(),0) );
const int HPanWidth = H11.Width()-HPanOffset;
HPan.LockedView( H, 0, H00.Width()+HPanOffset, HPanHeight, HPanWidth );
RepartitionDown
( AT, A0,
/**/ /**/
A1,
AB, A2 );
ATop.View2x1( A0,
A1 );
HPan_MC_STAR.AlignWith( ATop );
Z_STAR_MR.AlignWith( ATop );
Z_STAR_VR.AlignWith( ATop );
Zeros( HPan.Width(), ATop.Width(), Z_STAR_MR );
Zeros( HPan.Width(), HPan.Width(), SInv_STAR_STAR );
//--------------------------------------------------------------------//
HPanCopy = HPan;
MakeTrapezoidal( RIGHT, UPPER, offset, HPanCopy );
SetDiagonalToOne( RIGHT, offset, HPanCopy );
HPan_VC_STAR = HPanCopy;
Syrk
( LOWER, TRANSPOSE,
R(1), HPan_VC_STAR.LockedLocalMatrix(),
R(0), SInv_STAR_STAR.LocalMatrix() );
SInv_STAR_STAR.SumOverGrid();
HalveMainDiagonal( SInv_STAR_STAR );
HPan_MC_STAR = HPanCopy;
LocalGemm
( TRANSPOSE, NORMAL, R(1), HPan_MC_STAR, ATop, R(0), Z_STAR_MR );
Z_STAR_VR.SumScatterFrom( Z_STAR_MR );
LocalTrsm
( LEFT, LOWER, NORMAL, NON_UNIT,
R(1), SInv_STAR_STAR, Z_STAR_VR );
Z_STAR_MR = Z_STAR_VR;
LocalGemm( NORMAL, NORMAL, R(-1), HPan_MC_STAR, Z_STAR_MR, R(1), ATop );
//--------------------------------------------------------------------//
HPan_MC_STAR.FreeAlignments();
Z_STAR_MR.FreeAlignments();
Z_STAR_VR.FreeAlignments();
SlideLockedPartitionDownDiagonal
( HTL, /**/ HTR, H00, H01, /**/ H02,
/**/ H10, H11, /**/ H12,
/*************/ /******************/
HBL, /**/ HBR, H20, H21, /**/ H22 );
SlidePartitionDown
( AT, A0,
A1,
/**/ /**/
AB, A2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
internal::ApplyPackedReflectorsLLVF
( Conjugation conjugation, int offset,
const DistMatrix<Complex<R>,MC,MR >& H,
const DistMatrix<Complex<R>,MD,STAR>& t,
DistMatrix<Complex<R>,MC,MR >& A )
{
#ifndef RELEASE
PushCallStack("internal::ApplyPackedReflectorsLLVF");
if( H.Grid() != t.Grid() || t.Grid() != A.Grid() )
throw std::logic_error
("{H,t,A} must be distributed over the same grid");
if( offset > 0 )
throw std::logic_error("Transforms cannot extend above matrix");
if( offset < -H.Height() )
throw std::logic_error("Transforms cannot extend below matrix");
if( H.Height() != A.Height() )
throw std::logic_error
("Height of transforms must equal height of target matrix");
if( t.Height() != H.DiagonalLength( offset ) )
throw std::logic_error("t must be the same length as H's offset diag.");
if( !t.AlignedWithDiagonal( H, offset ) )
throw std::logic_error("t must be aligned with H's 'offset' diagonal");
#endif
typedef Complex<R> C;
const Grid& g = H.Grid();
// Matrix views
DistMatrix<C,MC,MR>
HTL(g), HTR(g), H00(g), H01(g), H02(g), HPan(g), HPanCopy(g),
HBL(g), HBR(g), H10(g), H11(g), H12(g),
H20(g), H21(g), H22(g);
DistMatrix<C,MC,MR>
AT(g), A0(g),
AB(g), A1(g),
A2(g);
DistMatrix<C,MD,STAR>
tT(g), t0(g),
tB(g), t1(g),
t2(g);
DistMatrix<C,VC, STAR> HPan_VC_STAR(g);
DistMatrix<C,MC, STAR> HPan_MC_STAR(g);
DistMatrix<C,STAR,STAR> t1_STAR_STAR(g);
DistMatrix<C,STAR,STAR> SInv_STAR_STAR(g);
DistMatrix<C,STAR,MR > Z_STAR_MR(g);
DistMatrix<C,STAR,VR > Z_STAR_VR(g);
LockedPartitionDownDiagonal
( H, HTL, HTR,
HBL, HBR, 0 );
LockedPartitionDown
( t, tT,
tB, 0 );
PartitionDown
( A, AT,
AB, 0 );
while( HTL.Height() < H.Height() && HTL.Width() < H.Width() )
{
LockedRepartitionDownDiagonal
( HTL, /**/ HTR, H00, /**/ H01, H02,
/*************/ /******************/
/**/ H10, /**/ H11, H12,
HBL, /**/ HBR, H20, /**/ H21, H22 );
int HPanHeight = H11.Height() + H21.Height();
int HPanWidth = std::min( H11.Width(), std::max(HPanHeight+offset,0) );
HPan.LockedView( H, H00.Height(), H00.Width(), HPanHeight, HPanWidth );
LockedRepartitionDown
( tT, t0,
/**/ /**/
t1,
tB, t2, HPanWidth );
RepartitionDown
( AT, A0,
/**/ /**/
A1,
AB, A2 );
HPan_MC_STAR.AlignWith( AB );
Z_STAR_MR.AlignWith( AB );
Z_STAR_VR.AlignWith( AB );
Z_STAR_MR.ResizeTo( HPan.Width(), AB.Width() );
SInv_STAR_STAR.ResizeTo( HPan.Width(), HPan.Width() );
Zero( SInv_STAR_STAR );
//--------------------------------------------------------------------//
HPanCopy = HPan;
MakeTrapezoidal( LEFT, LOWER, offset, HPanCopy );
SetDiagonalToOne( LEFT, offset, HPanCopy );
HPan_VC_STAR = HPanCopy;
Herk
( UPPER, ADJOINT,
(C)1, HPan_VC_STAR.LockedLocalMatrix(),
(C)0, SInv_STAR_STAR.LocalMatrix() );
SInv_STAR_STAR.SumOverGrid();
t1_STAR_STAR = t1;
FixDiagonal( conjugation, t1_STAR_STAR, SInv_STAR_STAR );
HPan_MC_STAR = HPanCopy;
internal::LocalGemm
( ADJOINT, NORMAL, (C)1, HPan_MC_STAR, AB, (C)0, Z_STAR_MR );
Z_STAR_VR.SumScatterFrom( Z_STAR_MR );
internal::LocalTrsm
( LEFT, UPPER, ADJOINT, NON_UNIT, (C)1, SInv_STAR_STAR, Z_STAR_VR );
Z_STAR_MR = Z_STAR_VR;
internal::LocalGemm
( NORMAL, NORMAL, (C)-1, HPan_MC_STAR, Z_STAR_MR, (C)1, AB );
//--------------------------------------------------------------------//
HPan_MC_STAR.FreeAlignments();
Z_STAR_MR.FreeAlignments();
Z_STAR_VR.FreeAlignments();
SlideLockedPartitionDownDiagonal
( HTL, /**/ HTR, H00, H01, /**/ H02,
/**/ H10, H11, /**/ H12,
/*************/ /******************/
HBL, /**/ HBR, H20, H21, /**/ H22 );
SlideLockedPartitionDown
( tT, t0,
t1,
/**/ /**/
tB, t2 );
SlidePartitionDown
( AT, A0,
A1,
/**/ /**/
AB, A2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
RLHF
( Conjugation conjugation, int offset,
const DistMatrix<Complex<R> >& H,
const DistMatrix<Complex<R>,MD,STAR>& t,
DistMatrix<Complex<R> >& A )
{
#ifndef RELEASE
PushCallStack("apply_packed_reflectors::RLHF");
if( H.Grid() != t.Grid() || t.Grid() != A.Grid() )
throw std::logic_error
("{H,t,A} must be distributed over the same grid");
if( offset > 0 || offset < -H.Width() )
throw std::logic_error("Transforms out of bounds");
if( H.Width() != A.Width() )
throw std::logic_error
("Width of transforms must equal width of target matrix");
if( t.Height() != H.DiagonalLength( offset ) )
throw std::logic_error("t must be the same length as H's offset diag");
if( !t.AlignedWithDiagonal( H, offset ) )
throw std::logic_error("t must be aligned with H's 'offset' diagonal");
#endif
typedef Complex<R> C;
const Grid& g = H.Grid();
DistMatrix<C>
HTL(g), HTR(g), H00(g), H01(g), H02(g), HPan(g), HPanCopy(g),
HBL(g), HBR(g), H10(g), H11(g), H12(g),
H20(g), H21(g), H22(g);
DistMatrix<C> ALeft(g);
DistMatrix<C,MD,STAR>
tT(g), t0(g),
tB(g), t1(g),
t2(g);
DistMatrix<C,STAR,VR > HPan_STAR_VR(g);
DistMatrix<C,STAR,MR > HPan_STAR_MR(g);
DistMatrix<C,STAR,STAR> t1_STAR_STAR(g);
DistMatrix<C,STAR,STAR> SInv_STAR_STAR(g);
DistMatrix<C,STAR,MC > ZAdj_STAR_MC(g);
DistMatrix<C,STAR,VC > ZAdj_STAR_VC(g);
LockedPartitionDownDiagonal
( H, HTL, HTR,
HBL, HBR, 0 );
LockedPartitionDown
( t, tT,
tB, 0 );
while( HTL.Height() < H.Height() && HTL.Width() < H.Width() )
{
LockedRepartitionDownDiagonal
( HTL, /**/ HTR, H00, /**/ H01, H02,
/*************/ /******************/
/**/ H10, /**/ H11, H12,
HBL, /**/ HBR, H20, /**/ H21, H22 );
const int HPanWidth = H10.Width() + H11.Width();
const int HPanOffset =
std::min( H11.Height(), std::max(-offset-H00.Height(),0) );
const int HPanHeight = H11.Height()-HPanOffset;
LockedView
( HPan, H, H00.Height()+HPanOffset, 0, HPanHeight, HPanWidth );
LockedRepartitionDown
( tT, t0,
/**/ /**/
t1,
tB, t2, HPanHeight );
View( ALeft, A, 0, 0, A.Height(), HPanWidth );
HPan_STAR_MR.AlignWith( ALeft );
ZAdj_STAR_MC.AlignWith( ALeft );
ZAdj_STAR_VC.AlignWith( ALeft );
Zeros( HPan.Height(), ALeft.Height(), ZAdj_STAR_MC );
Zeros( HPan.Height(), HPan.Height(), SInv_STAR_STAR );
//--------------------------------------------------------------------//
HPanCopy = HPan;
MakeTrapezoidal( RIGHT, LOWER, offset, HPanCopy );
SetDiagonal( RIGHT, offset, HPanCopy, C(1) );
HPan_STAR_VR = HPanCopy;
Herk
( UPPER, NORMAL,
C(1), HPan_STAR_VR.LockedMatrix(),
C(0), SInv_STAR_STAR.Matrix() );
SInv_STAR_STAR.SumOverGrid();
t1_STAR_STAR = t1;
FixDiagonal( conjugation, t1_STAR_STAR, SInv_STAR_STAR );
HPan_STAR_MR = HPan_STAR_VR;
LocalGemm
( NORMAL, ADJOINT,
C(1), HPan_STAR_MR, ALeft, C(0), ZAdj_STAR_MC );
ZAdj_STAR_VC.SumScatterFrom( ZAdj_STAR_MC );
LocalTrsm
( LEFT, UPPER, ADJOINT, NON_UNIT,
C(1), SInv_STAR_STAR, ZAdj_STAR_VC );
ZAdj_STAR_MC = ZAdj_STAR_VC;
LocalGemm
( ADJOINT, NORMAL,
C(-1), ZAdj_STAR_MC, HPan_STAR_MR, C(1), ALeft );
//--------------------------------------------------------------------//
HPan_STAR_MR.FreeAlignments();
ZAdj_STAR_MC.FreeAlignments();
ZAdj_STAR_VC.FreeAlignments();
SlideLockedPartitionDownDiagonal
( HTL, /**/ HTR, H00, H01, /**/ H02,
/**/ H10, H11, /**/ H12,
/*************/ /******************/
HBL, /**/ HBR, H20, H21, /**/ H22 );
SlideLockedPartitionDown
( tT, t0,
t1,
/**/ /**/
tB, t2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
RUVF
( Conjugation conjugation, Int offset,
const DistMatrix<F>& H, const DistMatrix<F,MD,STAR>& t, DistMatrix<F>& A )
{
#ifndef RELEASE
CallStackEntry cse("apply_packed_reflectors::RUVF");
if( H.Grid() != t.Grid() || t.Grid() != A.Grid() )
LogicError("{H,t,A} must be distributed over the same grid");
// TODO: Proper dimension checks
if( t.Height() != H.DiagonalLength(offset) )
LogicError("t must be the same length as H's offset diag");
if( !t.AlignedWithDiagonal( H, offset ) )
LogicError("t must be aligned with H's 'offset' diagonal");
#endif
const Grid& g = H.Grid();
DistMatrix<F>
HTL(g), HTR(g), H00(g), H01(g), H02(g), HPan(g), HPanCopy(g),
HBL(g), HBR(g), H10(g), H11(g), H12(g),
H20(g), H21(g), H22(g);
DistMatrix<F> ALeft(g);
DistMatrix<F,MD,STAR>
tT(g), t0(g),
tB(g), t1(g),
t2(g);
DistMatrix<F,VC, STAR> HPan_VC_STAR(g);
DistMatrix<F,MR, STAR> HPan_MR_STAR(g);
DistMatrix<F,STAR,STAR> t1_STAR_STAR(g);
DistMatrix<F,STAR,STAR> SInv_STAR_STAR(g);
DistMatrix<F,STAR,MC > ZAdj_STAR_MC(g);
DistMatrix<F,STAR,VC > ZAdj_STAR_VC(g);
LockedPartitionDownOffsetDiagonal
( offset,
H, HTL, HTR,
HBL, HBR, 0 );
LockedPartitionDown
( t, tT,
tB, 0 );
while( HTL.Height() < H.Height() && HTL.Width() < H.Width() )
{
LockedRepartitionDownDiagonal
( HTL, /**/ HTR, H00, /**/ H01, H02,
/*************/ /******************/
/**/ H10, /**/ H11, H12,
HBL, /**/ HBR, H20, /**/ H21, H22 );
LockedRepartitionDown
( tT, t0,
/**/ /**/
t1,
tB, t2 );
LockedView2x1( HPan, H01, H11 );
View( ALeft, A, 0, 0, A.Height(), HPan.Height() );
HPan_MR_STAR.AlignWith( ALeft );
ZAdj_STAR_MC.AlignWith( ALeft );
ZAdj_STAR_VC.AlignWith( ALeft );
//--------------------------------------------------------------------//
HPanCopy = HPan;
MakeTrapezoidal( UPPER, HPanCopy, 0, RIGHT );
SetDiagonal( HPanCopy, F(1), 0, RIGHT );
HPan_VC_STAR = HPanCopy;
Zeros( SInv_STAR_STAR, HPan.Width(), HPan.Width() );
Herk
( UPPER, ADJOINT,
F(1), HPan_VC_STAR.LockedMatrix(),
F(0), SInv_STAR_STAR.Matrix() );
SInv_STAR_STAR.SumOverGrid();
t1_STAR_STAR = t1;
FixDiagonal( conjugation, t1_STAR_STAR, SInv_STAR_STAR );
HPan_MR_STAR = HPan_VC_STAR;
LocalGemm( ADJOINT, ADJOINT, F(1), HPan_MR_STAR, ALeft, ZAdj_STAR_MC );
ZAdj_STAR_VC.SumScatterFrom( ZAdj_STAR_MC );
LocalTrsm
( LEFT, UPPER, ADJOINT, NON_UNIT,
F(1), SInv_STAR_STAR, ZAdj_STAR_VC );
ZAdj_STAR_MC = ZAdj_STAR_VC;
LocalGemm
( ADJOINT, ADJOINT, F(-1), ZAdj_STAR_MC, HPan_MR_STAR, F(1), ALeft );
//--------------------------------------------------------------------//
SlideLockedPartitionDownDiagonal
( HTL, /**/ HTR, H00, H01, /**/ H02,
/**/ H10, H11, /**/ H12,
/*************/ /******************/
HBL, /**/ HBR, H20, H21, /**/ H22 );
SlideLockedPartitionDown
( tT, t0,
t1,
/**/ /**/
tB, t2 );
}
}
// wrap forwardK
DLLIMPORT int get5AH01(double *qps, double *kinArray, double *arrayH01) {
int ret = -1;
FwdK5A fk(kinArray2Struct5A(kinArray));
Eigen::Matrix4d H01 = fk.qps2H01(qps);
arrayH01[0] = H01(0, 0);
arrayH01[1] = H01(1, 0);
arrayH01[2] = H01(2, 0);
arrayH01[3] = H01(0, 1);
arrayH01[4] = H01(1, 1);
arrayH01[5] = H01(2, 1);
arrayH01[6] = H01(0, 2);
arrayH01[7] = H01(1, 2);
arrayH01[8] = H01(2, 2);
arrayH01[9] = H01(0, 3);
arrayH01[10] = H01(1, 3);
arrayH01[11] = H01(2, 3);
ret = 0;
return ret;
}
inline void
RLHF
( int offset,
const DistMatrix<R>& H,
DistMatrix<R>& A )
{
#ifndef RELEASE
PushCallStack("apply_packed_reflectors::RLHF");
if( H.Grid() != A.Grid() )
throw std::logic_error("{H,A} must be distributed over the same grid");
if( offset > 0 || offset < -H.Width() )
throw std::logic_error("Transforms out of bounds");
if( H.Width() != A.Width() )
throw std::logic_error
("Width of transforms must equal width of target matrix");
#endif
const Grid& g = H.Grid();
DistMatrix<R>
HTL(g), HTR(g), H00(g), H01(g), H02(g), HPan(g), HPanCopy(g),
HBL(g), HBR(g), H10(g), H11(g), H12(g),
H20(g), H21(g), H22(g);
DistMatrix<R> ALeft(g);
DistMatrix<R,STAR,VR > HPan_STAR_VR(g);
DistMatrix<R,STAR,MR > HPan_STAR_MR(g);
DistMatrix<R,STAR,STAR> SInv_STAR_STAR(g);
DistMatrix<R,STAR,MC > ZTrans_STAR_MC(g);
DistMatrix<R,STAR,VC > ZTrans_STAR_VC(g);
LockedPartitionDownDiagonal
( H, HTL, HTR,
HBL, HBR, 0 );
while( HTL.Height() < H.Height() && HTL.Width() < H.Width() )
{
LockedRepartitionDownDiagonal
( HTL, /**/ HTR, H00, /**/ H01, H02,
/*************/ /******************/
/**/ H10, /**/ H11, H12,
HBL, /**/ HBR, H20, /**/ H21, H22 );
const int HPanWidth = H10.Width() + H11.Width();
const int HPanOffset =
std::min( H11.Height(), std::max(-offset-H00.Height(),0) );
const int HPanHeight = H11.Height()-HPanOffset;
LockedView
( HPan, H, H00.Height()+HPanOffset, 0, HPanHeight, HPanWidth );
View( ALeft, A, 0, 0, A.Height(), HPanWidth );
HPan_STAR_MR.AlignWith( ALeft );
ZTrans_STAR_MC.AlignWith( ALeft );
ZTrans_STAR_VC.AlignWith( ALeft );
Zeros( HPan.Height(), ALeft.Height(), ZTrans_STAR_MC );
Zeros( HPan.Height(), HPan.Height(), SInv_STAR_STAR );
//--------------------------------------------------------------------//
HPanCopy = HPan;
MakeTrapezoidal( RIGHT, LOWER, offset, HPanCopy );
SetDiagonal( RIGHT, offset, HPanCopy, R(1) );
HPan_STAR_VR = HPanCopy;
Syrk
( UPPER, NORMAL,
R(1), HPan_STAR_VR.LockedMatrix(),
R(0), SInv_STAR_STAR.Matrix() );
SInv_STAR_STAR.SumOverGrid();
HalveMainDiagonal( SInv_STAR_STAR );
HPan_STAR_MR = HPan_STAR_VR;
LocalGemm
( NORMAL, TRANSPOSE,
R(1), HPan_STAR_MR, ALeft, R(0), ZTrans_STAR_MC );
ZTrans_STAR_VC.SumScatterFrom( ZTrans_STAR_MC );
LocalTrsm
( LEFT, UPPER, TRANSPOSE, NON_UNIT,
R(1), SInv_STAR_STAR, ZTrans_STAR_VC );
ZTrans_STAR_MC = ZTrans_STAR_VC;
LocalGemm
( TRANSPOSE, NORMAL,
R(-1), ZTrans_STAR_MC, HPan_STAR_MR, R(1), ALeft );
//--------------------------------------------------------------------//
HPan_STAR_MR.FreeAlignments();
ZTrans_STAR_MC.FreeAlignments();
ZTrans_STAR_VC.FreeAlignments();
SlideLockedPartitionDownDiagonal
( HTL, /**/ HTR, H00, H01, /**/ H02,
/**/ H10, H11, /**/ H12,
/*************/ /******************/
HBL, /**/ HBR, H20, H21, /**/ H22 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
DLLIMPORT int get6AH01(double *qps, double *kinArray, double *arrayH01) {
int ret = -1;
FwdK6A fk(kinArray2Struct6A(kinArray));
Eigen::Matrix4d H01 = fk.qps2H01(qps); // non-static usage;
//Eigen::Matrix4d H01;
//H01 = FwdK::qps2H01(qps, params); //now static
//arrayH01 = H01.data(); lv can't read it..
arrayH01[0] = H01(0, 0);
arrayH01[1] = H01(1, 0);
arrayH01[2] = H01(2, 0);
arrayH01[3] = H01(0, 1);
arrayH01[4] = H01(1, 1);
arrayH01[5] = H01(2, 1);
arrayH01[6] = H01(0, 2);
arrayH01[7] = H01(1, 2);
arrayH01[8] = H01(2, 2);
arrayH01[9] = H01(0, 3);
arrayH01[10] = H01(1, 3);
arrayH01[11] = H01(2, 3);
ret = 0;
return ret;
}
returnValue CondensingExport::setupMultiplicationRoutines( )
{
ExportVariable QQ = deepcopy( Q );
QQ.setDataStruct( ACADO_LOCAL );
ExportVariable QC1( "QC1", getNX(),getNX() );
ExportVariable QE1( "QE1", getNX(),getNU()*getN() );
ExportVariable Dx1( "Dx1", getNX(),1 );
ExportVariable QDx1("QDx1", getNX(),1 );
ExportVariable Du1( "Du1", getNU(),1 );
ExportVariable RDu1("RDu1", getNU(),1 );
ExportVariable Gx ( "Gx", getNX(),getNX() );
ExportVariable Gu ( "Gu", getNX(),getNU() );
ExportVariable C1 ( "C1", getNX(),getNX() );
ExportVariable E1 ( "E1", getNX(),getNU()*getN() );
ExportVariable d1 ( "d1", getNX(),1 );
ExportVariable u1 ( "u1", getNU(),1 );
ExportVariable C ( "C", getNX()*getN(),getNX() );
ExportVariable QC ( "QC", getNX()*getN(),getNX() );
ExportVariable E ( "E", getNX()*getN(),getNU()*getN() );
ExportVariable QE ( "QE", getNX()*getN(),getNU()*getN() );
ExportVariable QDx( "QDx", getNX()*getN(),1 );
ExportVariable g0 ( "g0", getNX(),1 );
ExportVariable g1 ( "g1", getNU()*getN(),1 );
ExportVariable H00( "H00", getNX(),getNX() );
ExportVariable H01( "H01", getNX(),getNU()*getN() );
ExportVariable H11( "H11", getNU()*getN(),getNU()*getN() );
multiplyQC1.setup( "multiplyQC1", QQ,C1,QC1 );
multiplyQC1.addStatement( QC1 == QQ*C1 );
multiplyQE1.setup( "multiplyQE1", QQ,E1,QE1 );
multiplyQE1.addStatement( QE1 == QQ*E1 );
multiplyQDX1.setup( "multiplyQDX1", QQ,Dx1,QDx1 );
multiplyQDX1.addStatement( QDx1 == QQ*Dx1 );
multiplyRDU1.setup( "multiplyRDU1", R,Du1,RDu1 );
multiplyRDU1.addStatement( RDu1 == R*Du1 );
multiplyQC2.setup( "multiplyQC2", QQF,C1,QC1 );
multiplyQC2.addStatement( QC1 == QQF*C1 );
multiplyQE2.setup( "multiplyQE2", QQF,E1,QE1 );
multiplyQE2.addStatement( QE1 == QQF*E1 );
multiplyQDX2.setup( "multiplyQDX2", QQF,Dx1,QDx1 );
multiplyQDX2.addStatement( QDx1 == QQF*Dx1 );
multiplyC.setup( "multiplyC", Gx,C1,C1("C1_new") );
multiplyC.addStatement( C1("C1_new") == Gx*C1 );
multiplyE.setup( "multiplyE", Gx,E1,E1("E1_new") );
multiplyE.addStatement( E1("E1_new") == Gx*E1 );
if ( performsSingleShooting() == BT_FALSE )
{
multiplyCD1.setup( "multiplyCD1", Gx,d1,d1("d1_new") );
multiplyCD1.addStatement( d1("d1_new") == Gx*d1 );
multiplyEU1.setup( "multiplyEU1", Gu,u1,d1("d1_new") );
multiplyEU1.addStatement( d1("d1_new") += Gu*u1 );
}
if ( performsFullCondensing() == BT_FALSE )
{
multiplyG0.setup( "multiplyG0", C,QDx,g0 );
multiplyG0.addStatement( g0 == (C^QDx) );
}
multiplyG1.setup( "multiplyG1", E,QDx,g1 );
multiplyG1.addStatement( g1 == (E^QDx) );
if ( performsFullCondensing() == BT_FALSE )
{
multiplyH00.setup( "multiplyH00", C,QC,H00 );
multiplyH00.addStatement( H00 == (C^QC) );
}
multiplyH01.setup( "multiplyH01", C,QE,H01 );
multiplyH01.addStatement( H01 == (C^QE) );
multiplyH11.setup( "multiplyH11", E,QE,H11 );
multiplyH11.addStatement( (H11 == (E^QE)) );
return SUCCESSFUL_RETURN;
}
inline void
LUHF
( Conjugation conjugation, Int offset,
const DistMatrix<F>& H, const DistMatrix<F,MD,STAR>& t, DistMatrix<F>& A )
{
#ifndef RELEASE
CallStackEntry cse("apply_packed_reflectors::LUHF");
if( H.Grid() != t.Grid() || t.Grid() != A.Grid() )
LogicError("{H,t,A} must be distributed over the same grid");
// TODO: Proper dimension checks
if( t.Height() != H.DiagonalLength(offset) )
LogicError("t must be the same length as H's offset diag");
if( !t.AlignedWithDiagonal( H, offset ) )
LogicError("t must be aligned with H's offset diagonal");
#endif
const Grid& g = H.Grid();
DistMatrix<F>
HTL(g), HTR(g), H00(g), H01(g), H02(g), HPan(g), HPanCopy(g),
HBL(g), HBR(g), H10(g), H11(g), H12(g),
H20(g), H21(g), H22(g);
DistMatrix<F>
AT(g), A0(g),
AB(g), A1(g),
A2(g);
DistMatrix<F,MD,STAR>
tT(g), t0(g),
tB(g), t1(g),
t2(g);
DistMatrix<F,STAR,VR > HPan_STAR_VR(g);
DistMatrix<F,STAR,MC > HPan_STAR_MC(g);
DistMatrix<F,STAR,STAR> t1_STAR_STAR(g);
DistMatrix<F,STAR,STAR> SInv_STAR_STAR(g);
DistMatrix<F,STAR,MR > Z_STAR_MR(g);
DistMatrix<F,STAR,VR > Z_STAR_VR(g);
LockedPartitionDownOffsetDiagonal
( offset,
H, HTL, HTR,
HBL, HBR, 0 );
LockedPartitionDown
( t, tT,
tB, 0 );
PartitionDown
( A, AT,
AB, 0 );
while( HTL.Height() < H.Height() && HTL.Width() < H.Width() )
{
LockedRepartitionDownDiagonal
( HTL, /**/ HTR, H00, /**/ H01, H02,
/*************/ /******************/
/**/ H10, /**/ H11, H12,
HBL, /**/ HBR, H20, /**/ H21, H22 );
LockedRepartitionDown
( tT, t0,
/**/ /**/
t1,
tB, t2 );
RepartitionDown
( AT, A0,
/**/ /**/
A1,
AB, A2, H11.Height() );
LockedView1x2( HPan, H11, H12 );
HPan_STAR_MC.AlignWith( AB );
Z_STAR_MR.AlignWith( AB );
Z_STAR_VR.AlignWith( AB );
//--------------------------------------------------------------------//
HPanCopy = HPan;
MakeTriangular( UPPER, HPanCopy );
SetDiagonal( HPanCopy, F(1) );
HPan_STAR_VR = HPanCopy;
Zeros( SInv_STAR_STAR, HPan.Height(), HPan.Height() );
Herk
( LOWER, NORMAL,
F(1), HPan_STAR_VR.LockedMatrix(),
F(0), SInv_STAR_STAR.Matrix() );
SInv_STAR_STAR.SumOverGrid();
t1_STAR_STAR = t1;
FixDiagonal( conjugation, t1_STAR_STAR, SInv_STAR_STAR );
HPan_STAR_MC = HPan_STAR_VR;
LocalGemm( NORMAL, NORMAL, F(1), HPan_STAR_MC, AB, Z_STAR_MR );
Z_STAR_VR.SumScatterFrom( Z_STAR_MR );
LocalTrsm
( LEFT, LOWER, NORMAL, NON_UNIT, F(1), SInv_STAR_STAR, Z_STAR_VR );
Z_STAR_MR = Z_STAR_VR;
LocalGemm( ADJOINT, NORMAL, F(-1), HPan_STAR_MC, Z_STAR_MR, F(1), AB );
//--------------------------------------------------------------------//
SlideLockedPartitionDownDiagonal
( HTL, /**/ HTR, H00, H01, /**/ H02,
/**/ H10, H11, /**/ H12,
/*************/ /******************/
HBL, /**/ HBR, H20, H21, /**/ H22 );
SlideLockedPartitionDown
( tT, t0,
t1,
/**/ /**/
tB, t2 );
SlidePartitionDown
( AT, A0,
A1,
/**/ /**/
AB, A2 );
}
}
inline void
ApplyPackedReflectorsLUHB
( int offset,
const DistMatrix<R>& H,
DistMatrix<R>& A )
{
#ifndef RELEASE
PushCallStack("internal::ApplyPackedReflectorsLUHB");
if( H.Grid() != A.Grid() )
throw std::logic_error("{H,A} must be distributed over the same grid");
if( offset < 0 || offset > H.Width() )
throw std::logic_error("Transforms out of bounds");
if( H.Width() != A.Height() )
throw std::logic_error
("Width of transforms must equal height of target matrix");
#endif
const Grid& g = H.Grid();
DistMatrix<R>
HTL(g), HTR(g), H00(g), H01(g), H02(g), HPan(g), HPanCopy(g),
HBL(g), HBR(g), H10(g), H11(g), H12(g),
H20(g), H21(g), H22(g);
DistMatrix<R> ABottom(g);
DistMatrix<R,STAR,VR > HPan_STAR_VR(g);
DistMatrix<R,STAR,MC > HPan_STAR_MC(g);
DistMatrix<R,STAR,STAR> SInv_STAR_STAR(g);
DistMatrix<R,STAR,MR > Z_STAR_MR(g);
DistMatrix<R,STAR,VR > Z_STAR_VR(g);
LockedPartitionUpDiagonal
( H, HTL, HTR,
HBL, HBR, 0 );
while( HBR.Height() < H.Height() && HBR.Width() < H.Width() )
{
LockedRepartitionUpDiagonal
( HTL, /**/ HTR, H00, H01, /**/ H02,
/**/ H10, H11, /**/ H12,
/*************/ /******************/
HBL, /**/ HBR, H20, H21, /**/ H22 );
const int HPanWidth = H11.Width() + H12.Width();
const int HPanHeight =
std::min( H11.Height(), std::max(HPanWidth-offset,0) );
const int leftover = A.Height()-HPanWidth;
HPan.LockedView( H, H00.Height(), H00.Width(), HPanHeight, HPanWidth );
ABottom.View( A, leftover, 0, HPanWidth, A.Width() );
HPan_STAR_MC.AlignWith( ABottom );
Z_STAR_MR.AlignWith( ABottom );
Z_STAR_VR.AlignWith( ABottom );
Zeros( HPanHeight, ABottom.Width(), Z_STAR_MR );
Zeros( HPanHeight, HPanHeight, SInv_STAR_STAR );
//--------------------------------------------------------------------//
HPanCopy = HPan;
MakeTrapezoidal( LEFT, UPPER, offset, HPanCopy );
SetDiagonalToOne( LEFT, offset, HPanCopy );
HPan_STAR_VR = HPanCopy;
Syrk
( UPPER, NORMAL,
R(1), HPan_STAR_VR.LockedLocalMatrix(),
R(0), SInv_STAR_STAR.LocalMatrix() );
SInv_STAR_STAR.SumOverGrid();
HalveMainDiagonal( SInv_STAR_STAR );
HPan_STAR_MC = HPan_STAR_VR;
LocalGemm
( NORMAL, NORMAL, R(1), HPan_STAR_MC, ABottom, R(0), Z_STAR_MR );
Z_STAR_VR.SumScatterFrom( Z_STAR_MR );
LocalTrsm
( LEFT, UPPER, NORMAL, NON_UNIT, R(1), SInv_STAR_STAR, Z_STAR_VR );
Z_STAR_MR = Z_STAR_VR;
LocalGemm
( TRANSPOSE, NORMAL, R(-1), HPan_STAR_MC, Z_STAR_MR, R(1), ABottom );
//--------------------------------------------------------------------//
HPan_STAR_MC.FreeAlignments();
Z_STAR_MR.FreeAlignments();
Z_STAR_VR.FreeAlignments();
SlideLockedPartitionUpDiagonal
( HTL, /**/ HTR, H00, /**/ H01, H02,
/*************/ /******************/
/**/ H10, /**/ H11, H12,
HBL, /**/ HBR, H20, /**/ H21, H22 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
ApplyPackedReflectorsLUHB
( Conjugation conjugation, int offset,
const DistMatrix<Complex<R> >& H,
const DistMatrix<Complex<R>,MD,STAR>& t,
DistMatrix<Complex<R> >& A )
{
#ifndef RELEASE
PushCallStack("internal::ApplyPackedReflectorsLUHB");
if( H.Grid() != t.Grid() || t.Grid() != A.Grid() )
throw std::logic_error
("{H,t,A} must be distributed over the same grid");
if( offset < 0 || offset > H.Width() )
throw std::logic_error("Transforms out of bounds");
if( H.Width() != A.Height() )
throw std::logic_error
("Width of transforms must equal height of target matrix");
if( t.Height() != H.DiagonalLength( offset ) )
throw std::logic_error("t must be the same length as H's offset diag");
if( !t.AlignedWithDiagonal( H, offset ) )
throw std::logic_error("t must be aligned with H's offset diagonal");
#endif
typedef Complex<R> C;
const Grid& g = H.Grid();
DistMatrix<C>
HTL(g), HTR(g), H00(g), H01(g), H02(g), HPan(g), HPanCopy(g),
HBL(g), HBR(g), H10(g), H11(g), H12(g),
H20(g), H21(g), H22(g);
DistMatrix<C> ABottom(g);
DistMatrix<C,MD,STAR>
tT(g), t0(g),
tB(g), t1(g),
t2(g);
DistMatrix<C,STAR,VR > HPan_STAR_VR(g);
DistMatrix<C,STAR,MC > HPan_STAR_MC(g);
DistMatrix<C,STAR,STAR> t1_STAR_STAR(g);
DistMatrix<C,STAR,STAR> SInv_STAR_STAR(g);
DistMatrix<C,STAR,MR > Z_STAR_MR(g);
DistMatrix<C,STAR,VR > Z_STAR_VR(g);
LockedPartitionUpDiagonal
( H, HTL, HTR,
HBL, HBR, 0 );
LockedPartitionUp
( t, tT,
tB, 0 );
while( HBR.Height() < H.Height() && HBR.Width() < H.Width() )
{
LockedRepartitionUpDiagonal
( HTL, /**/ HTR, H00, H01, /**/ H02,
/**/ H10, H11, /**/ H12,
/*************/ /******************/
HBL, /**/ HBR, H20, H21, /**/ H22 );
const int HPanWidth = H11.Width() + H12.Width();
const int HPanHeight =
std::min( H11.Height(), std::max(HPanWidth-offset,0) );
const int leftover = A.Height()-HPanWidth;
HPan.LockedView( H, H00.Height(), H00.Width(), HPanHeight, HPanWidth );
LockedRepartitionUp
( tT, t0,
t1,
/**/ /**/
tB, t2, HPanHeight );
ABottom.View( A, leftover, 0, HPanWidth, A.Width() );
HPan_STAR_MC.AlignWith( ABottom );
Z_STAR_MR.AlignWith( ABottom );
Z_STAR_VR.AlignWith( ABottom );
Zeros( HPanHeight, ABottom.Width(), Z_STAR_MR );
Zeros( HPanHeight, HPanHeight, SInv_STAR_STAR );
//--------------------------------------------------------------------//
HPanCopy = HPan;
MakeTrapezoidal( LEFT, UPPER, offset, HPanCopy );
SetDiagonalToOne( LEFT, offset, HPanCopy );
HPan_STAR_VR = HPanCopy;
Herk
( UPPER, NORMAL,
C(1), HPan_STAR_VR.LockedLocalMatrix(),
C(0), SInv_STAR_STAR.LocalMatrix() );
SInv_STAR_STAR.SumOverGrid();
t1_STAR_STAR = t1;
FixDiagonal( conjugation, t1_STAR_STAR, SInv_STAR_STAR );
HPan_STAR_MC = HPan_STAR_VR;
LocalGemm
( NORMAL, NORMAL, C(1), HPan_STAR_MC, ABottom, C(0), Z_STAR_MR );
Z_STAR_VR.SumScatterFrom( Z_STAR_MR );
LocalTrsm
( LEFT, UPPER, NORMAL, NON_UNIT, C(1), SInv_STAR_STAR, Z_STAR_VR );
Z_STAR_MR = Z_STAR_VR;
LocalGemm
( ADJOINT, NORMAL, C(-1), HPan_STAR_MC, Z_STAR_MR, C(1), ABottom );
//--------------------------------------------------------------------//
HPan_STAR_MC.FreeAlignments();
Z_STAR_MR.FreeAlignments();
Z_STAR_VR.FreeAlignments();
SlideLockedPartitionUpDiagonal
( HTL, /**/ HTR, H00, /**/ H01, H02,
/*************/ /******************/
/**/ H10, /**/ H11, H12,
HBL, /**/ HBR, H20, /**/ H21, H22 );
SlideLockedPartitionUp
( tT, t0,
/**/ /**/
t1,
tB, t2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
