Foam::tmp<Foam::pointField> Foam::RBD::rigidBodyMotion::transformPoints
(
const label bodyID,
const pointField& initialPoints
) const
{
// Calculate the transform from the initial state in the global frame
// to the current state in the global frame
spatialTransform X(X0(bodyID).inv() & X00(bodyID));
tmp<pointField> tpoints(new pointField(initialPoints.size()));
pointField& points = tpoints.ref();
forAll(points, i)
{
points[i] = X.transformPoint(initialPoints[i]);
}
inline void
TrtrsmLLN
( UnitOrNonUnit diag, F alpha, const DistMatrix<F>& L, DistMatrix<F>& X,
bool checkIfSingular )
{
#ifndef RELEASE
CallStackEntry entry("internal::TrtrsmLLN");
#endif
const Grid& g = L.Grid();
// Matrix views
DistMatrix<F>
LTL(g), LTR(g), L00(g), L01(g), L02(g),
LBL(g), LBR(g), L10(g), L11(g), L12(g),
L20(g), L21(g), L22(g);
DistMatrix<F>
XTL(g), XTR(g), X00(g), X01(g), X02(g),
XBL(g), XBR(g), X10(g), X11(g), X12(g),
X20(g), X21(g), X22(g);
// Temporary distributions
DistMatrix<F,STAR,STAR> L11_STAR_STAR(g);
DistMatrix<F,MC, STAR> L21_MC_STAR(g);
DistMatrix<F,STAR,MR > X10_STAR_MR(g);
DistMatrix<F,STAR,VR > X10_STAR_VR(g);
DistMatrix<F,STAR,MR > X11_STAR_MR(g);
DistMatrix<F,STAR,STAR> X11_STAR_STAR(g);
// Start the algorithm
ScaleTrapezoid( alpha, LOWER, X );
LockedPartitionDownDiagonal
( L, LTL, LTR,
LBL, LBR, 0 );
PartitionDownDiagonal
( X, XTL, XTR,
XBL, XBR, 0 );
while( XBR.Height() > 0 )
{
LockedRepartitionDownDiagonal
( LTL, /**/ LTR, L00, /**/ L01, L02,
/*************/ /******************/
/**/ L10, /**/ L11, L12,
LBL, /**/ LBR, L20, /**/ L21, L22 );
RepartitionDownDiagonal
( XTL, /**/ XTR, X00, /**/ X01, X02,
/*************/ /******************/
/**/ X10, /**/ X11, X12,
XBL, /**/ XBR, X20, /**/ X21, X22 );
L21_MC_STAR.AlignWith( X20 );
X10_STAR_MR.AlignWith( X20 );
X11_STAR_MR.AlignWith( X21 );
//--------------------------------------------------------------------//
L11_STAR_STAR = L11;
X11_STAR_STAR = X11;
X10_STAR_VR = X10;
LocalTrsm
( LEFT, LOWER, NORMAL, diag, F(1), L11_STAR_STAR, X10_STAR_VR,
checkIfSingular );
LocalTrtrsm
( LEFT, LOWER, NORMAL, diag, F(1), L11_STAR_STAR, X11_STAR_STAR,
checkIfSingular );
X11 = X11_STAR_STAR;
X11_STAR_MR = X11_STAR_STAR;
MakeTriangular( LOWER, X11_STAR_MR );
X10_STAR_MR = X10_STAR_VR;
X10 = X10_STAR_MR;
L21_MC_STAR = L21;
LocalGemm
( NORMAL, NORMAL, F(-1), L21_MC_STAR, X10_STAR_MR, F(1), X20 );
LocalGemm
( NORMAL, NORMAL, F(-1), L21_MC_STAR, X11_STAR_MR, F(1), X21 );
//--------------------------------------------------------------------//
SlideLockedPartitionDownDiagonal
( LTL, /**/ LTR, L00, L01, /**/ L02,
/**/ L10, L11, /**/ L12,
/*************/ /******************/
LBL, /**/ LBR, L20, L21, /**/ L22 );
SlidePartitionDownDiagonal
( XTL, /**/ XTR, X00, X01, /**/ X02,
/**/ X10, X11, /**/ X12,
/*************/ /******************/
XBL, /**/ XBR, X20, X21, /**/ X22 );
}
}
