//------------------------------------------------------------------------------
// Rmatrix33 LowerRight()
//------------------------------------------------------------------------------
Rmatrix33 Rmatrix66::LowerRight()
{
Rmatrix33 mat33(elementD[21], elementD[22], elementD[23],
elementD[27], elementD[28], elementD[29],
elementD[33], elementD[34], elementD[35]);
return mat33;
}
//------------------------------------------------------------------------------
// Rmatrix33 UpperRight()
//------------------------------------------------------------------------------
Rmatrix33 Rmatrix66::UpperRight()
{
Rmatrix33 mat33(elementD[3], elementD[4], elementD[5],
elementD[9], elementD[10], elementD[11],
elementD[15], elementD[16], elementD[17]);
return mat33;
}
//------------------------------------------------------------------------------
// Rmatrix33 LowerLeft()
//------------------------------------------------------------------------------
Rmatrix33 Rmatrix66::LowerLeft()
{
Rmatrix33 mat33(elementD[18], elementD[19], elementD[20],
elementD[24], elementD[25], elementD[26],
elementD[30], elementD[31], elementD[32]);
return mat33;
}
//------------------------------------------------------------------------------
// Rmatrix33 UpperLeft()
//------------------------------------------------------------------------------
Rmatrix33 Rmatrix66::UpperLeft()
{
Rmatrix33 mat33(elementD[0], elementD[1], elementD[2],
elementD[6], elementD[7], elementD[8],
elementD[12], elementD[13], elementD[14]);
return mat33;
}
void TestDim::index() {
Dim sca;
Dim row2(Dim::row_vec(2));
Dim row3(Dim::row_vec(3));
Dim col2(Dim::col_vec(2));
Dim col3(Dim::col_vec(3));
Dim mat22(Dim::matrix(2,2));
Dim mat32(Dim::matrix(3,2));
Dim mat23(Dim::matrix(2,3));
Dim mat33(Dim::matrix(3,3));
CPPUNIT_ASSERT (sca.index_dim(DoubleIndex::one_elt(sca,0,0))==sca);
CPPUNIT_ASSERT (row3.index_dim(DoubleIndex::one_elt(row3,0,0))==sca);
CPPUNIT_ASSERT (row3.index_dim(DoubleIndex::one_row(row3,0))==row3);
CPPUNIT_ASSERT (row3.index_dim(DoubleIndex::cols(row3,0,1))==row2);
CPPUNIT_ASSERT (col3.index_dim(DoubleIndex::one_elt(col3,0,0))==sca);
CPPUNIT_ASSERT (col3.index_dim(DoubleIndex::one_col(col3,0))==col3);
CPPUNIT_ASSERT (col3.index_dim(DoubleIndex::rows(col3,0,1))==col2);
CPPUNIT_ASSERT (mat23.index_dim(DoubleIndex::one_elt(mat23,0,0))==sca);
CPPUNIT_ASSERT (mat23.index_dim(DoubleIndex::one_row(mat23,0))==row3);
CPPUNIT_ASSERT (mat23.index_dim(DoubleIndex::one_col(mat23,0))==col2);
CPPUNIT_ASSERT (mat23.index_dim(DoubleIndex::cols(mat23,0,1))==mat22);
CPPUNIT_ASSERT (mat23.index_dim(DoubleIndex::subrow(mat23,0,0,1))==row2);
CPPUNIT_ASSERT (mat23.index_dim(DoubleIndex::all(mat23))==mat23);
CPPUNIT_ASSERT (mat32.index_dim(DoubleIndex::subcol(mat32,0,1,0))==col2);
CPPUNIT_ASSERT (mat33.index_dim(DoubleIndex::rows(mat33,0,1))==mat23);
CPPUNIT_ASSERT (mat33.index_dim(DoubleIndex::cols(mat33,0,1))==mat32);
}
const mat33 mat33_FromColMajor(const real data0, const real data1, const real data2,
const real data3, const real data4, const real data5,
const real data6, const real data7, const real data8)
{
return mat33(data0, data3, data6,
data1, data4, data7,
data2, data5, data8);
}
MathLib::Matrix4 toMatrix4(const tf::Pose& pose) {
MathLib::Matrix4 mat;
mat.Identity();
tf::Matrix3x3 mat33(pose.getRotation());
mat.SetTranslation(MathLib::Vector3(pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()));
mat.SetOrientation(MathLib::Matrix3(mat33[0][0], mat33[0][1], mat33[0][2],
mat33[1][0], mat33[1][1], mat33[1][2],
mat33[2][0], mat33[2][1], mat33[2][2]));
return mat;
}
const mat33 cmat33(const quat& q)
{
const real x = X(q);
const real y = Y(q);
const real z = Z(q);
const real w = W(q);
const real tXX = 2 * Square(x);
const real tYY = 2 * Square(y);
const real tZZ = 2 * Square(z);
const real tXY = 2 * x * y;
const real tYZ = 2 * y * z;
const real tZW = 2 * z * w;
const real tXW = 2 * x * w;
const real tXZ = 2 * x * z;
const real tYW = 2 * y * w;
return mat33(
1 - tYY - tZZ, tXY - tZW, tXZ + tYW,
tXY + tZW, 1 - tXX - tZZ, tYZ - tXW,
tXZ - tYW, tYZ + tXW, 1 - tXX - tYY);
}
void TestDim::vec() {
Dim sca;
Dim row2(Dim::row_vec(2));
Dim row3(Dim::row_vec(3));
Dim col2(Dim::col_vec(2));
Dim col3(Dim::col_vec(3));
Dim mat22(Dim::matrix(2,2));
Dim mat32(Dim::matrix(3,2));
Dim mat23(Dim::matrix(2,3));
Dim mat33(Dim::matrix(3,3));
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(sca),true)==sca);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(sca),false)==sca);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(sca,sca),true)==row2);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(sca,sca),false)==col2);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(sca,sca,sca),true)==row3);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(sca,sca,sca),false)==col3);
CPPUNIT_ASSERT_THROW(vec_dim(Array<const Dim>(sca,col2),true),DimException);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(sca,col2),false)==col3);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(sca,row2),true)==row3);
CPPUNIT_ASSERT_THROW(vec_dim(Array<const Dim>(sca,row2),false),DimException);
CPPUNIT_ASSERT_THROW(vec_dim(Array<const Dim>(sca,mat22),true),DimException);
CPPUNIT_ASSERT_THROW(vec_dim(Array<const Dim>(sca,mat22),false),DimException);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(col2),true)==col2);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(col2),false)==col2);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(col2,col2),true)==mat22);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(col2,col2),false)==Dim::col_vec(4));
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(col2,col2,col2),true)==mat23);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(col2,col2,col2),false)==Dim::col_vec(6));
CPPUNIT_ASSERT_THROW(vec_dim(Array<const Dim>(col2,row2),true),DimException);
CPPUNIT_ASSERT_THROW(vec_dim(Array<const Dim>(col2,row2),false),DimException);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(col2,mat22),true)==mat23);
CPPUNIT_ASSERT_THROW(vec_dim(Array<const Dim>(col2,mat22),false),DimException);
CPPUNIT_ASSERT (vec_dim(Array<const Dim>(col2,mat22,col2),true)==Dim::matrix(2,4));
}
const mat33 cmat33(const mat44& m)
{
return mat33(m(0, 0), m(0, 1), m(0, 2),
m(1, 0), m(1, 1), m(1, 2),
m(2, 0), m(2, 1), m(2, 2));
}
const mat33 Scale(const vec3& scale)
{
return mat33(X(scale), 0, 0,
0, Y(scale), 0,
0, 0, Z(scale));
}
void TestDim::mul() {
Dim sca;
Dim row2(Dim::row_vec(2));
Dim row3(Dim::row_vec(3));
Dim col2(Dim::col_vec(2));
Dim col3(Dim::col_vec(3));
Dim mat22(Dim::matrix(2,2));
Dim mat32(Dim::matrix(3,2));
Dim mat23(Dim::matrix(2,3));
Dim mat33(Dim::matrix(3,3));
CPPUNIT_ASSERT (mul_dim(sca,sca)==sca);
CPPUNIT_ASSERT (mul_dim(sca,row2)==row2);
CPPUNIT_ASSERT (mul_dim(sca,row3)==row3);
CPPUNIT_ASSERT (mul_dim(sca,col2)==col2);
CPPUNIT_ASSERT (mul_dim(sca,col3)==col3);
CPPUNIT_ASSERT (mul_dim(sca,mat22)==mat22);
CPPUNIT_ASSERT (mul_dim(sca,mat23)==mat23);
CPPUNIT_ASSERT (mul_dim(sca,mat32)==mat32);
CPPUNIT_ASSERT (mul_dim(sca,mat33)==mat33);
CPPUNIT_ASSERT_THROW(mul_dim(row2,sca),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(row2,row2),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(row2,row3),DimException);
CPPUNIT_ASSERT (mul_dim(row2,col2)==sca);
CPPUNIT_ASSERT_THROW(mul_dim(row2,col3),DimException);
CPPUNIT_ASSERT (mul_dim(row2,mat22)==row2);
CPPUNIT_ASSERT_THROW(mul_dim(row2,mat32),DimException);
CPPUNIT_ASSERT (mul_dim(row2,mat23)==row3);
CPPUNIT_ASSERT_THROW(mul_dim(row2,mat33),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(row3,sca),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(row3,row2),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(row3,row3),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(row3,col2),DimException);
CPPUNIT_ASSERT (mul_dim(row3,col3)==sca);
CPPUNIT_ASSERT_THROW(mul_dim(row3,mat22),DimException);
CPPUNIT_ASSERT (mul_dim(row3,mat32)==row2);
CPPUNIT_ASSERT_THROW(mul_dim(row3,mat23),DimException);
CPPUNIT_ASSERT (mul_dim(row3,mat33)==row3);
CPPUNIT_ASSERT (mul_dim(col2,sca)==col2);
CPPUNIT_ASSERT (mul_dim(col2,row2)==mat22);
CPPUNIT_ASSERT (mul_dim(col2,row3)==mat23);
CPPUNIT_ASSERT_THROW(mul_dim(col2,col2),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(col2,col3),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(col2,mat22),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(col2,mat32),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(col2,mat23),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(col2,mat33),DimException);
CPPUNIT_ASSERT (mul_dim(col3,sca)==col3);
CPPUNIT_ASSERT (mul_dim(col3,row2)==mat32);
CPPUNIT_ASSERT (mul_dim(col3,row3)==mat33);
CPPUNIT_ASSERT_THROW(mul_dim(col3,col2),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(col3,col3),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(col3,mat22),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(col3,mat32),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(col3,mat23),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(col3,mat33),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat22,sca),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat22,row2),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat22,row3),DimException);
CPPUNIT_ASSERT (mul_dim(mat22,col2)==col2);
CPPUNIT_ASSERT_THROW(mul_dim(mat22,col3),DimException);
CPPUNIT_ASSERT (mul_dim(mat22,mat22)==mat22);
CPPUNIT_ASSERT_THROW(mul_dim(mat22,mat32),DimException);
CPPUNIT_ASSERT (mul_dim(mat22,mat23)==mat23);
CPPUNIT_ASSERT_THROW(mul_dim(mat22,mat33),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat32,sca),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat32,row2),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat32,row3),DimException);
CPPUNIT_ASSERT (mul_dim(mat32,col2)==col3);
CPPUNIT_ASSERT_THROW(mul_dim(mat32,col3),DimException);
CPPUNIT_ASSERT (mul_dim(mat32,mat22)==mat32);
CPPUNIT_ASSERT_THROW(mul_dim(mat32,mat32),DimException);
CPPUNIT_ASSERT (mul_dim(mat32,mat23)==mat33);
CPPUNIT_ASSERT_THROW(mul_dim(mat32,mat33),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat23,sca),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat23,row2),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat23,row3),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat23,col2),DimException);
CPPUNIT_ASSERT (mul_dim(mat23,col3)==col2);
CPPUNIT_ASSERT_THROW(mul_dim(mat23,mat22),DimException);
CPPUNIT_ASSERT (mul_dim(mat23,mat32)==mat22);
CPPUNIT_ASSERT_THROW(mul_dim(mat23,mat23),DimException);
CPPUNIT_ASSERT (mul_dim(mat23,mat33)==mat23);
CPPUNIT_ASSERT_THROW(mul_dim(mat33,sca),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat33,row2),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat33,row3),DimException);
CPPUNIT_ASSERT_THROW(mul_dim(mat33,col2),DimException);
CPPUNIT_ASSERT (mul_dim(mat33,col3)==col3);
CPPUNIT_ASSERT_THROW(mul_dim(mat33,mat22),DimException);
CPPUNIT_ASSERT (mul_dim(mat33,mat32)==mat32);
CPPUNIT_ASSERT_THROW(mul_dim(mat33,mat23),DimException);
CPPUNIT_ASSERT (mul_dim(mat33,mat33)==mat33);
}
mat33 MatrixStack::normalMatrix()
{
return mat33(inverse().transpose());
}