Verified Commit 508d2887 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

joints/constraint: set correct return type for basic Constraint operations

parent 714ae40e
......@@ -102,6 +102,26 @@ namespace pinocchio
}; // class ConstraintBase
/// \brief Operation Y * S used in the CRBA algorithm for instance
template<typename Scalar, int Options, typename ConstraintDerived>
typename MultiplicationOp<InertiaTpl<Scalar,Options>,ConstraintDerived>::ReturnType
operator*(const InertiaTpl<Scalar,Options> & Y,
const ConstraintBase<ConstraintDerived> & constraint)
{
return impl::LhsMultiplicationOp<InertiaTpl<Scalar,Options>,ConstraintDerived>::run(Y,
constraint.derived());
}
/// \brief Operation Y_matrix * S used in the ABA algorithm for instance
template<typename MatrixDerived, typename ConstraintDerived>
typename MultiplicationOp<Eigen::MatrixBase<MatrixDerived>,ConstraintDerived>::ReturnType
operator*(const Eigen::MatrixBase<MatrixDerived> & Y,
const ConstraintBase<ConstraintDerived> & constraint)
{
return impl::LhsMultiplicationOp<Eigen::MatrixBase<MatrixDerived>,ConstraintDerived>::run(Y.derived(),
constraint.derived());
}
} // namespace pinocchio
......
......@@ -158,6 +158,14 @@ namespace pinocchio
typedef typename MotionDerived::MotionPlain ReturnType;
return ReturnType(m1.rate*m1.axis + m2.linear(), m2.angular());
}
template<typename MotionDerived, typename S2, int O2>
inline typename MotionDerived::MotionPlain
operator^(const MotionDense<MotionDerived> & m1,
const MotionPrismaticUnalignedTpl<S2,O2> & m2)
{
return m2.motionAction(m1);
}
template<typename Scalar, int Options> struct ConstraintPrismaticUnalignedTpl;
......@@ -245,27 +253,16 @@ namespace pinocchio
TransposeConst(const ConstraintPrismaticUnalignedTpl & ref) : ref(ref) {}
template<typename ForceDerived>
<<<<<<< HEAD
typename ConstraintForceOp<ConstraintPrismaticUnaligned,ForceDerived>::ReturnType
operator* (const ForceDense<ForceDerived> & f) const
{
typedef typename ConstraintForceOp<ConstraintPrismaticUnaligned,ForceDerived>::ReturnType ReturnType;
=======
typename internal::ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType
operator* (const ForceDense<ForceDerived> & f) const
{
typedef typename internal::ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType ReturnType;
>>>>>>> joint/prismatic: add missing Tpl in the naming
return ReturnType(ref.axis.dot(f.linear()));
}
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
template<typename ForceSet>
<<<<<<< HEAD
typename ConstraintForceSetOp<ConstraintPrismaticUnaligned,ForceSet>::ReturnType
=======
typename internal::ConstraintForceSetOp<ConstraintPrismaticUnalignedTpl,ForceSet>::ReturnType
>>>>>>> joint/prismatic: add missing Tpl in the naming
operator*(const Eigen::MatrixBase<ForceSet> & F)
{
EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
......@@ -303,44 +300,67 @@ namespace pinocchio
Vector3 axis;
}; // struct ConstraintPrismaticUnalignedTpl
template<typename MotionDerived, typename S2, int O2>
inline typename MotionDerived::MotionPlain
operator^(const MotionDense<MotionDerived> & m1,
const MotionPrismaticUnalignedTpl<S2,O2> & m2)
template<typename S1, int O1,typename S2, int O2>
struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticUnalignedTpl<S2,O2> >
{
return m2.motionAction(m1);
}
typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
};
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S1,6,1>
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismaticUnalignedTpl<S2,O2> & cpu)
namespace impl
{
typedef InertiaTpl<S1,O1> Inertia;
/* YS = [ m -mcx ; mcx I-mcxcx ] [ v ; 0 ] = [ mv ; mcxv ] */
const S1 & m = Y.mass();
const typename Inertia::Vector3 & c = Y.lever();
Eigen::Matrix<S1,6,1> res;
res.template head<3>().noalias() = m*cpu.axis;
res.template tail<3>() = c.cross(res.template head<3>());
return res;
}
template<typename S1, int O1, typename S2, int O2>
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticUnalignedTpl<S2,O2> >
{
typedef InertiaTpl<S1,O1> Inertia;
typedef ConstraintPrismaticUnalignedTpl<S2,O2> Constraint;
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
static inline ReturnType run(const Inertia & Y,
const Constraint & cpu)
{
ReturnType res;
/* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
const S1 & m = Y.mass();
const typename Inertia::Vector3 & c = Y.lever();
res.template head<3>().noalias() = m*cpu.axis;
res.template tail<3>().noalias() = c.cross(res.template head<3>());
return res;
}
};
} // namespace impl
/* [ABA] Y*S operator (Inertia Y,Constraint S) */
template<typename M6, typename S2, int O2>
const typename MatrixMatrixProduct<
Eigen::Block<const M6,6,3>,
typename ConstraintPrismaticUnalignedTpl<S2,O2>::Vector3
>::type
operator*(const Eigen::MatrixBase<M6> & Y, const ConstraintPrismaticUnalignedTpl<S2,O2> & cpu)
template<typename M6Like, typename Scalar, int Options>
struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6);
return Y.template block<6,3> (0,Inertia::LINEAR) * cpu.axis;
}
typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint;
typedef typename Constraint::Vector3 Vector3;
typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
};
/* [ABA] operator* (Inertia Y,Constraint S) */
namespace impl
{
template<typename M6Like, typename Scalar, int Options>
struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
{
typedef ConstraintPrismaticUnalignedTpl<Scalar,Options> Constraint;
typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
const Constraint & cru)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis;
}
};
} // namespace impl
template<typename Scalar, int Options> struct JointPrismaticUnalignedTpl;
template<typename _Scalar, int _Options>
......
......@@ -353,59 +353,108 @@ namespace pinocchio
}
}; // struct ConstraintPrismaticTpl
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S1,6,1,O1>
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismaticTpl<S2,O2,0> &)
{
/* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
const S1
&m = Y.mass(),
&y = Y.lever()[1],
&z = Y.lever()[2];
Eigen::Matrix<S1,6,1,O1> res;
res << m, S1(0), S1(0), S1(0), m*z, -m*y;
return res;
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S1,6,1,O1>
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismaticTpl<S2,O2,1> & )
{
/* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
const S1
&m = Y.mass(),
&x = Y.lever()[0],
&z = Y.lever()[2];
Eigen::Matrix<S1,6,1,O1> res;
res << S1(0), m, S1(0), -m*z, S1(0), m*x;
return res;
}
template<typename S1, int O1,typename S2, int O2, int axis>
struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,axis> >
{
typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
};
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S1,6,1,O1>
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPrismaticTpl<S2,O2,2> & )
{
/* Y(:,2) = ( 0,0, 1, y , -x , 0) */
const S1
&m = Y.mass(),
&x = Y.lever()[0],
&y = Y.lever()[1];
Eigen::Matrix<S1,6,1,O1> res;
res << S1(0), S1(0), m, m*y, -m*x, S1(0);
return res;
}
namespace impl
{
template<typename S1, int O1, typename S2, int O2>
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,0> >
{
typedef InertiaTpl<S1,O1> Inertia;
typedef ConstraintPrismaticTpl<S2,O2,0> Constraint;
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
static inline ReturnType run(const Inertia & Y,
const Constraint & /*constraint*/)
{
ReturnType res;
/* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
const S1
&m = Y.mass(),
&y = Y.lever()[1],
&z = Y.lever()[2];
res << m, S1(0), S1(0), S1(0), m*z, -m*y;
return res;
}
};
template<typename S1, int O1, typename S2, int O2>
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,1> >
{
typedef InertiaTpl<S1,O1> Inertia;
typedef ConstraintPrismaticTpl<S2,O2,1> Constraint;
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
static inline ReturnType run(const Inertia & Y,
const Constraint & /*constraint*/)
{
ReturnType res;
/* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
const S1
&m = Y.mass(),
&x = Y.lever()[0],
&z = Y.lever()[2];
res << S1(0), m, S1(0), -m*z, S1(0), m*x;
return res;
}
};
template<typename S1, int O1, typename S2, int O2>
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintPrismaticTpl<S2,O2,2> >
{
typedef InertiaTpl<S1,O1> Inertia;
typedef ConstraintPrismaticTpl<S2,O2,2> Constraint;
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
static inline ReturnType run(const Inertia & Y,
const Constraint & /*constraint*/)
{
ReturnType res;
/* Y(:,2) = ( 0,0, 1, y , -x , 0) */
const S1
&m = Y.mass(),
&x = Y.lever()[0],
&y = Y.lever()[1];
res << S1(0), S1(0), m, m*y, -m*x, S1(0);
return res;
}
};
} // namespace impl
template<typename M6Like,typename S2, int O2, int axis>
struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<S2,O2,axis> >
{
typedef typename M6Like::ConstColXpr ReturnType;
};
/* [ABA] operator* (Inertia Y,Constraint S) */
template<typename M6Like, typename S2, int O2, int axis>
inline const typename M6Like::ConstColXpr
operator*(const Eigen::MatrixBase<M6Like> & Y, const ConstraintPrismaticTpl<S2,O2,axis> &)
namespace impl
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
return Y.derived().col(Inertia::LINEAR + axis);
}
template<typename M6Like, typename Scalar, int Options, int axis>
struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<Scalar,Options,axis> >
{
typedef ConstraintPrismaticTpl<Scalar,Options,axis> Constraint;
typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
const Constraint & /*constraint*/)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
return Y.derived().col(Inertia::LINEAR + axis);
}
};
} // namespace impl
template<typename _Scalar, int _Options, int _axis>
struct JointPrismaticTpl
{
......
......@@ -163,12 +163,21 @@ namespace pinocchio
template<typename S1, int O1, typename MotionDerived>
inline typename MotionDerived::MotionPlain
operator+(const MotionRevoluteUnalignedTpl<S1,O1> & m1, const MotionDense<MotionDerived> & m2)
operator+(const MotionRevoluteUnalignedTpl<S1,O1> & m1,
const MotionDense<MotionDerived> & m2)
{
typename MotionDerived::MotionPlain res(m2);
res += m1;
return res;
}
template<typename MotionDerived, typename S2, int O2>
inline typename MotionDerived::MotionPlain
operator^(const MotionDense<MotionDerived> & m1,
const MotionRevoluteUnalignedTpl<S2,O2> & m2)
{
return m2.motionAction(m1);
}
template<typename Scalar, int Options> struct ConstraintRevoluteUnalignedTpl;
......@@ -312,47 +321,69 @@ namespace pinocchio
}; // struct ConstraintRevoluteUnalignedTpl
template<typename MotionDerived, typename S2, int O2>
inline typename MotionDerived::MotionPlain
operator^(const MotionDense<MotionDerived> & m1, const MotionRevoluteUnalignedTpl<S2,O2> & m2)
template<typename S1, int O1,typename S2, int O2>
struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteUnalignedTpl<S2,O2> >
{
/* m1xm2 = [ v1xw2 + w1xv2; w1xw2 ] = [ v1xw2; w1xw2 ] */
return m2.motionAction(m1);
}
typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
};
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S2,6,1,O2>
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintRevoluteUnalignedTpl<S2,O2> & cru)
namespace impl
{
typedef InertiaTpl<S1,O1> Inertia;
/* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
const typename Inertia::Scalar & m = Y.mass();
const typename Inertia::Vector3 & c = Y.lever();
const typename Inertia::Symmetric3 & I = Y.inertia();
Eigen::Matrix<S2,6,1,O2>res;
res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis);
res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis;
res.template segment<3>(Inertia::ANGULAR) += c.cross(res.template segment<3>(Inertia::LINEAR));
return res;
}
/* [ABA] Y*S operator (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2, int O2>
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteUnalignedTpl<S2,O2> >
{
typedef InertiaTpl<S1,O1> Inertia;
typedef ConstraintRevoluteUnalignedTpl<S2,O2> Constraint;
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
static inline ReturnType run(const Inertia & Y,
const Constraint & cru)
{
ReturnType res;
/* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
const typename Inertia::Scalar & m = Y.mass();
const typename Inertia::Vector3 & c = Y.lever();
const typename Inertia::Symmetric3 & I = Y.inertia();
res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis);
res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis;
res.template segment<3>(Inertia::ANGULAR) += c.cross(res.template segment<3>(Inertia::LINEAR));
return res;
}
};
} // namespace impl
template<typename M6Like, typename S2, int O2>
inline
const typename MatrixMatrixProduct<
typename Eigen::internal::remove_const<typename SizeDepType<3>::ColsReturn<M6Like>::ConstType>::type,
typename ConstraintRevoluteUnalignedTpl<S2,O2>::Vector3
>::type
operator*(const Eigen::MatrixBase<M6Like> & Y,
const ConstraintRevoluteUnalignedTpl<S2,O2> & cru)
template<typename M6Like, typename Scalar, int Options>
struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> >
{
typedef ConstraintRevoluteUnalignedTpl<S2,O2> Constraint;
return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis;
}
typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint;
typedef typename Constraint::Vector3 Vector3;
typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
};
/* [ABA] operator* (Inertia Y,Constraint S) */
namespace impl
{
template<typename M6Like, typename Scalar, int Options>
struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> >
{
typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint;
typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
const Constraint & cru)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis;
}
};
} // namespace impl
template<typename Scalar, int Options> struct JointRevoluteUnalignedTpl;
template<typename _Scalar, int _Options>
......
......@@ -432,76 +432,132 @@ namespace pinocchio
axis = _axis
};
};
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S2,6,1,O2>
operator*(const InertiaTpl<S1,O1> & Y,const ConstraintRevoluteTpl<S2,O2,0> &)
template<typename S1, int O1,typename S2, int O2, int axis>
struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,axis> >
{
typedef InertiaTpl<S1,O1> Inertia;
/* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */
const S1
&m = Y.mass(),
&x = Y.lever()[0],
&y = Y.lever()[1],
&z = Y.lever()[2];
const typename Inertia::Symmetric3 & I = Y.inertia();
Eigen::Matrix<S2,6,1,O2> res;
res << (S2)0,-m*z,m*y,
I(0,0)+m*(y*y+z*z),
I(0,1)-m*x*y,
I(0,2)-m*x*z ;
return res;
}
typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
};
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S2,6,1,O2>
operator*(const InertiaTpl<S1,O1> & Y,const ConstraintRevoluteTpl<S2,O2,1> &)
namespace impl
{
typedef InertiaTpl<S1,O1> Inertia;
/* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */
const S1
&m = Y.mass(),
&x = Y.lever()[0],
&y = Y.lever()[1],
&z = Y.lever()[2];
const typename Inertia::Symmetric3 & I = Y.inertia();
Eigen::Matrix<S2,6,1,O2> res;
res << m*z,(S2)0,-m*x,
I(1,0)-m*x*y,
I(1,1)+m*(x*x+z*z),
I(1,2)-m*y*z ;
return res;
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
template<typename S1, int O1, typename S2, int O2>
inline Eigen::Matrix<S2,6,1,O2>
operator*(const InertiaTpl<S1,O1> & Y,const ConstraintRevoluteTpl<S2,O2,2> &)
template<typename S1, int O1, typename S2, int O2>
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,0> >
{
typedef InertiaTpl<S1,O1> Inertia;
typedef ConstraintRevoluteTpl<S2,O2,0> Constraint;
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
static inline ReturnType run(const Inertia & Y,
const Constraint & /*constraint*/)
{
ReturnType res;
/* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */
const S1
&m = Y.mass(),
&x = Y.lever()[0],
&y = Y.lever()[1],
&z = Y.lever()[2];
const typename Inertia::Symmetric3 & I = Y.inertia();
res <<
(S2)0,
-m*z,
m*y,
I(0,0)+m*(y*y+z*z),
I(0,1)-m*x*y,
I(0,2)-m*x*z;
return res;
}
};
template<typename S1, int O1, typename S2, int O2>
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteTpl<S2,O2,1> >
{
typedef InertiaTpl<S1,O1> Inertia;
typedef ConstraintRevoluteTpl<S2,O2,1> Constraint;
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
static inline ReturnType run(const Inertia & Y,
const Constraint & /*constraint*/)
{
ReturnType res;
/* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */
const S1
&m = Y.mass(),
&x = Y.lever()[0],
&y = Y.lever()[1],
&z = Y.lever()[2];
const typename Inertia::Symmetric3 & I = Y.inertia();
res <<
m*z,
(S2)0,
-m*x,
I(1,0)-m*x*y,
I(1,1)+m*(x*x+z*z),
I(1,2)-m*y*z;