void SmrCCDSolver::computeErrorVect(void)
{
if ( m_jointProcessed == m_IKChainPtr->getNumJoints() ) m_jointProcessed = 0;
SmrKinematicJoint *currentJoint = m_IKChainPtr->getJoint(m_jointProcessed);
//change this according to constraints.
// Build target vector
ColumnVector targetVector(3);
ColumnVector currentVector(3);
m_IKChainPtr->setMode( ABSOLUTEMODE );
SmrIKConstraint* currentConstraint = getConstraintPtr(0);
//get the related joint for every constraint
SmrJoint* relatedJoint = currentConstraint->getRelatedJointPtr();
SmrQuaternion relatedOrient = relatedJoint->getOrient();
SmrVector3 currentOffset = currentConstraint->getOffset();
//apply the local offsets defined in the constraint
relatedOrient.rotate(currentOffset);
// update error vector
currentVector(1) = (relatedJoint->getPos()+currentOffset).X();
currentVector(2) = (relatedJoint->getPos()+currentOffset).Y();
currentVector(3) = (relatedJoint->getPos()+currentOffset).Z();
targetVector(1) = (currentConstraint->getPosition()).X();
targetVector(2) = (currentConstraint->getPosition()).Y();
targetVector(3) = (currentConstraint->getPosition()).Z();
//cout << currentOffset << endl;
m_IKChainPtr->setMode( RELATIVEMODE );
//cout << m_error << endl;// << currentVector << endl << targetVector << endl;
currentVector-=targetVector;
m_error=currentVector;
//cout << m_IKChainPtr->getName() << " errorCalculated" << endl;
}
OrthogonalProjections::OrthogonalProjections(const Matrix& originalVectors,
Real multiplierCutoff,
Real tolerance)
: originalVectors_(originalVectors),
multiplierCutoff_(multiplierCutoff),
numberVectors_(originalVectors.rows()),
dimension_(originalVectors.columns()),
validVectors_(true,originalVectors.rows()), // opposite way round from vector constructor
orthoNormalizedVectors_(originalVectors.rows(),
originalVectors.columns())
{
std::vector<Real> currentVector(dimension_);
for (Size j=0; j < numberVectors_; ++j)
{
if (validVectors_[j])
{
for (Size k=0; k< numberVectors_; ++k) // create an orthormal basis not containing j
{
for (Size m=0; m < dimension_; ++m)
orthoNormalizedVectors_[k][m] = originalVectors_[k][m];
if ( k !=j && validVectors_[k])
{
for (Size l=0; l < k; ++l)
{
if (validVectors_[l] && l !=j)
{
Real dotProduct = innerProduct(orthoNormalizedVectors_, k, orthoNormalizedVectors_,l);
for (Size n=0; n < dimension_; ++n)
orthoNormalizedVectors_[k][n] -= dotProduct*orthoNormalizedVectors_[l][n];
}
}
Real normBeforeScaling= norm(orthoNormalizedVectors_,k);
if (normBeforeScaling < tolerance)
{
validVectors_[k] = false;
}
else
{
Real normBeforeScalingRecip = 1.0/normBeforeScaling;
for (Size m=0; m < dimension_; ++m)
orthoNormalizedVectors_[k][m] *= normBeforeScalingRecip;
} // end of else (norm < tolerance)
} // end of if k !=j && validVectors_[k])
}// end of for (Size k=0; k< numberVectors_; ++k)
// we now have an o.n. basis for everything except j
Real prevNormSquared = normSquared(originalVectors_, j);
for (Size r=0; r < numberVectors_; ++r)
if (validVectors_[r] && r != j)
{
Real dotProduct = innerProduct(orthoNormalizedVectors_, j, orthoNormalizedVectors_,r);
for (Size s=0; s < dimension_; ++s)
orthoNormalizedVectors_[j][s] -= dotProduct*orthoNormalizedVectors_[r][s];
}
Real projectionOnOriginalDirection = innerProduct(originalVectors_,j,orthoNormalizedVectors_,j);
Real sizeMultiplier = prevNormSquared/projectionOnOriginalDirection;
if (std::fabs(sizeMultiplier) < multiplierCutoff_)
{
for (Size t=0; t < dimension_; ++t)
currentVector[t] = orthoNormalizedVectors_[j][t]*sizeMultiplier;
}
else
validVectors_[j] = false;
} // end of if (validVectors_[j])
projectedVectors_.push_back(currentVector);
} //end of j loop
numberValidVectors_ =0;
for (Size i=0; i < numberVectors_; ++i)
numberValidVectors_ += validVectors_[i] ? 1 : 0;
} // end of constructor
