Constraint::SphereOnSphereContact::SphereOnSphereContact
(MobilizedBody& mobod_F,
const Vec3& defaultCenter_F,
Real defaultRadius_F,
MobilizedBody& mobod_B,
const Vec3& defaultCenter_B,
Real defaultRadius_B,
bool enforceRolling)
: Constraint(new SphereOnSphereContactImpl(enforceRolling))
{
SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSubsystem() && mobod_B.isInSubsystem(),
"Constraint::SphereOnSphereContact","SphereOnSphereContact",
"Both mobilized bodies must already be in a SimbodyMatterSubsystem.");
SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSameSubsystem(mobod_B),
"Constraint::SphereOnSphereContact","SphereOnSphereContact",
"The two mobilized bodies to be connected must be in the same "
"SimbodyMatterSubsystem.");
SimTK_APIARGCHECK2_ALWAYS(defaultRadius_F > 0 && defaultRadius_B > 0,
"Constraint::SphereOnSphereContact","SphereOnSphereContact",
"The sphere radii must be greater than zero; they were %g and %g.",
defaultRadius_F, defaultRadius_B);
mobod_F.updMatterSubsystem().adoptConstraint(*this);
updImpl().m_mobod_F = updImpl().addConstrainedBody(mobod_F);
updImpl().m_mobod_B = updImpl().addConstrainedBody(mobod_B);
updImpl().m_def_p_FSf = defaultCenter_F;
updImpl().m_def_radius_F = defaultRadius_F;
updImpl().m_def_p_BSb = defaultCenter_B;
updImpl().m_def_radius_B = defaultRadius_B;
}
void Differentiator::calcJacobian
(const Vector& y0, const Vector& fy0, Matrix& dfdy,
Differentiator::Method m) const
{
rep->nDifferentiations++;
rep->nDifferentiationFailures++; // assume the worst
SimTK_APIARGCHECK2_ALWAYS(y0.size()==rep->NParameters, "Differentiator", "calcJacobian",
"Expecting %d elements in the parameter (state) vector but got %d",
rep->NParameters, (int)y0.size());
SimTK_APIARGCHECK2_ALWAYS(fy0.size()==rep->NFunctions, "Differentiator", "calcJacobian",
"Expecting %d elements in the unperturbed function value but got %d",
rep->NFunctions, (int)fy0.size());
rep->frep.calcJacobian(*rep,m,y0,&fy0,dfdy);
rep->nDifferentiationFailures--;
}
void FactorQTZRep<T>::solve( const Matrix_<T>& b, Matrix_<T>& x ) const {
SimTK_APIARGCHECK_ALWAYS(isFactored ,"FactorQTZ","solve",
"No matrix was passed to FactorQTZ. \n" );
SimTK_APIARGCHECK2_ALWAYS(b.nrow()==nRow,"FactorQTZ","solve",
"number of rows in right hand side=%d does not match number of rows in original matrix=%d \n",
b.nrow(), nRow );
x.resize(nCol, b.ncol() );
Matrix_<T> tb;
tb.resize(maxmn, b.ncol() );
for(int j=0;j<b.ncol();j++) for(int i=0;i<b.nrow();i++) tb(i,j) = b(i,j);
doSolve(tb, x);
}
// The slow version
Matrix Differentiator::calcJacobian
(const Vector& y0, Differentiator::Method m) const
{
rep->nDifferentiations++;
rep->nDifferentiationFailures++; // assume the worst
SimTK_APIARGCHECK2_ALWAYS(y0.size()==rep->NParameters, "Differentiator", "calcJacobian",
"Expecting %d elements in the parameter (state) vector but got %d",
rep->NParameters, (int)y0.size());
Matrix dfdy(rep->NFunctions, rep->NParameters);
rep->frep.calcJacobian(*rep,m,y0,0,dfdy);
rep->nDifferentiationFailures--;
return dfdy;
}
// The slow version
Vector Differentiator::calcGradient
(const Vector& y0, Differentiator::Method m) const
{
rep->nDifferentiations++;
rep->nDifferentiationFailures++; // assume the worst
SimTK_APIARGCHECK2_ALWAYS(y0.size()==rep->NParameters, "Differentiator", "calcGradient",
"Supplied number of parameters (state length) was %ld but should have been %d",
y0.size(), rep->NParameters);
Vector grad(rep->NParameters);
rep->frep.calcGradient(*rep,m,y0,0,grad);
rep->nDifferentiationFailures--;
return grad; // TODO: use DeadVector to avoid copy
}
void FactorQTZRep<T>::solve( const Vector_<T>& b, Vector_<T> &x ) const {
SimTK_APIARGCHECK_ALWAYS(isFactored ,"FactorQTZ","solve",
"No matrix was passed to FactorQTZ. \n" );
SimTK_APIARGCHECK2_ALWAYS(b.size()==nRow,"FactorQTZ","solve",
"number of rows in right hand side=%d does not match number of rows in original matrix=%d \n",
b.size(), nRow );
Matrix_<T> m(maxmn,1);
for(int i=0;i<b.size();i++) {
m(i,0) = b(i);
}
Matrix_<T> r(nCol, 1 );
doSolve( m, r );
x.copyAssign(r);
}
void FactorQTZRep<T>::factor(const Matrix_<ELT>&mat ) {
SimTK_APIARGCHECK2_ALWAYS(mat.nelt() > 0,"FactorQTZ","factor",
"Can't factor a matrix that has a zero dimension -- got %d X %d.",
(int)mat.nrow(), (int)mat.ncol());
// allocate and initialize the matrix we pass to LAPACK
// converts (negated,conjugated etc.) to LAPACK format
LapackConvert::convertMatrixToLapack( qtz.data, mat );
int info;
const int smallestSingularValue = 2;
const int largestSingularValue = 1;
typedef typename CNT<T>::TReal RealType;
RealType smlnum, bignum, smin, smax;
if (mat.nelt() == 0) return;
// Compute optimal size for work space for dtzrzf and dgepq3. The
// arguments here should match the calls below, although we'll use maxRank
// rather than rank since we don't know the rank yet.
T workSz;
const int maxRank = std::min(nRow, nCol);
LapackInterface::tzrzf<T>(maxRank, nCol, 0, nRow, 0, &workSz, -1, info);
const int lwork1 = (int)NTraits<T>::real(workSz);
LapackInterface::geqp3<T>(nRow, nCol, 0, nRow, 0, 0, &workSz, -1, info);
const int lwork2 = (int)NTraits<T>::real(workSz);
TypedWorkSpace<T> work(std::max(lwork1, lwork2));
LapackInterface::getMachinePrecision<RealType>( smlnum, bignum);
// scale the input system of equations
anrm = (RealType)LapackInterface::lange<T>('M', nRow, nCol, qtz.data, nRow);
if (anrm > 0 && anrm < smlnum) {
scaleLinSys = true;
linSysScaleF = smlnum;
} else if( anrm > bignum ) {
scaleLinSys = true;
linSysScaleF = bignum;
}
if (anrm == 0) { // matrix all zeros
rank = 0;
} else {
if (scaleLinSys) {
LapackInterface::lascl<T>('G', 0, 0, anrm, linSysScaleF, nRow, nCol,
qtz.data, nRow, info );
}
// compute QR factorization with column pivoting: A = Q * R
// Q * R is returned in qtz.data
LapackInterface::geqp3<T>(nRow, nCol, qtz.data, nRow, pivots.data,
tauGEQP3.data, work.data, work.size, info );
// compute Rank
smax = CNT<T>::abs( qtz.data[0] );
smin = smax;
if( CNT<T>::abs(qtz.data[0]) == 0 ) {
rank = 0;
actualRCond = 0;
} else {
T s1,s2,c1,c2;
RealType smaxpr,sminpr;
// Determine rank using incremental condition estimate
Array_<T> xSmall(mn), xLarge(mn); // temporaries
xSmall[0] = xLarge[0] = 1;
for (rank=1,smaxpr=0.0,sminpr=1.0;
rank<mn && smaxpr*rcond < sminpr; )
{
LapackInterface::laic1<T>(smallestSingularValue, rank,
xSmall.begin(), smin, &qtz.data[rank*nRow],
qtz.data[(rank*nRow)+rank], sminpr, s1, c1);
LapackInterface::laic1<T>(largestSingularValue, rank,
xLarge.begin(), smax, &qtz.data[rank*nRow],
qtz.data[(rank*nRow)+rank], smaxpr, s2, c2);
if (smaxpr*rcond < sminpr) {
for(int i=0; i<rank; i++) {
xSmall[i] *= s1;
xLarge[i] *= s2;
}
xSmall[rank] = c1;
xLarge[rank] = c2;
smin = sminpr;
smax = smaxpr;
actualRCond = (double)(smin/smax);
rank++;
}
}
// R => T * Z
// Matrix is rank deficient so complete the orthogonalization by
// applying orthogonal transformations from the right to
// factor R from an upper trapezoidal to an upper triangular matrix
// T is returned in qtz.data and Z is returned in tauORMQR.data
if (rank < nCol) {
LapackInterface::tzrzf<T>(rank, nCol, qtz.data, nRow,
tauORMQR.data, work.data,
work.size, info);
}
}
}
}
