C++ (Cpp) Rotation::reexpressSymMat33 Examples

Example #1

File: RotationTest.cpp Project: BrianZ1/simbody

//-------------------------------------------------------------------
// Test the tricked-out code used to rotation a symmetric dyadic
// matrix S_AA=R_AB*S_BB*R_BA.
bool testReexpressSymMat33() {
    bool test = true;

    const Rotation R_AB(Test::randRotation());
    const SymMat33 S_BB(Test::randSymMat<3>());
    const Mat33 M_BB(S_BB);

    const SymMat33 S_AA = R_AB.reexpressSymMat33(S_BB);
    const Mat33 M_AA = R_AB*M_BB*~R_AB;

    const Mat33 MS_AA(S_AA);
    test = test && (MS_AA-M_AA).norm() <= SignificantReal;

    // Now put it back with an InverseRotation.
    const SymMat33 isS_BB = (~R_AB).reexpressSymMat33(S_AA);
    test = test && (S_BB-isS_BB).norm() <= SignificantReal;

    const Rotation I; // identity
    const SymMat33 S_BB_still = I.reexpressSymMat33(S_BB);
    test = test && (S_BB_still-S_BB).norm() <= SignificantReal;

    // Test symmetric matrix multiply (doesn't belong here).
    const SymMat33 S1(Test::randSymMat<3>()), S2(Test::randSymMat<3>());
    const Mat33 M1(S1), M2(S2);
    const Mat33 S(S1*S2);
    const Mat33 M(M1*M2);
    test = test && (S-M).norm() <= SignificantReal;

    const SymMat<3,Complex> SC1(Test::randComplex(),
                                Test::randComplex(), Test::randComplex(),
                                Test::randComplex(), Test::randComplex(), Test::randComplex() );
    const SymMat<3,Complex> SC2(Test::randComplex(),
                                Test::randComplex(), Test::randComplex(),
                                Test::randComplex(), Test::randComplex(), Test::randComplex() );

    SimTK_TEST_EQ(SC1.elt(1,0), conj(SC1.elt(0,1)));
    SimTK_TEST_EQ(SC1.elt(2,0), conj(SC1.elt(0,2)));
    SimTK_TEST_EQ(SC1.elt(1,2), conj(SC1.elt(2,1)));

    const Mat<3,3,Complex> MC1(SC1), MC2(SC2);
    const Mat<3,3,Complex> SC(SC1*SC2);
    const Mat<3,3,Complex> MC(MC1*MC2);
    SimTK_TEST_EQ(SC, MC);

    return test;
}