Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add null space task for IK.

Browse Source
This commit is contained in:
yunfeibai
2016-09-29 17:12:51 -07:00
parent 0ee12475af
commit 94c7bbe8e3
3 changed files with 39 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
View File

@@ -973,6 +973,21 @@ bool MatrixRmn::DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const Matr
return ret;
}
bool MatrixRmn::DebugCheckInverse( const MatrixRmn& MInv ) const
{
assert ( this->NumRows==this->NumCols );
assert ( MInv.NumRows==MInv.NumCols );
MatrixRmn I(this->NumRows, this->NumCols);
I.SetIdentity();
MatrixRmn MMInv(this->NumRows, this->NumCols);
Multiply(*this, MInv, MMInv);
I -= MMInv;
double error = I.FrobeniusNorm();
bool ret = ( fabs(error)<=1.0e-13 );
assert ( ret );
return ret;
}
bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const
{
// Special SVD test code