mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-08-08 09:49:03 +08:00
* revert the previous interface change in solveTriangular (pointer vs reference)
* remove the cast operators in the Geometry module: they are replaced by constructors and new operator= in Matrix * extended the operations supported by Rotation2D * rewrite in solveTriangular: - merge the Upper and Lower specializations - big optimization of the path for row-major triangular matrices
This commit is contained in:
Gael Guennebaud
parent
e778ae2559
commit
95dd09bea6
@ -149,7 +149,7 @@ typename Derived::Eval CholeskyWithoutSquareRoot<MatrixType>::solve(const Matrix
|
||||
|
||||
return m_matrix.adjoint().template part<UnitUpper>()
|
||||
.solveTriangular(
|
||||
( m_matrix.cwise().inverse().diagonal().asDiagonal()
|
||||
( m_matrix.cwise().inverse().template part<Diagonal>()
|
||||
* matrixL().solveTriangular(b))
|
||||
);
|
||||
}
|
||||
|
||||
@ -363,6 +363,13 @@ class Matrix : public MatrixBase<Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCol
|
||||
else
|
||||
this->Base::swap(other);
|
||||
}
|
||||
|
||||
/////////// Geometry module ///////////
|
||||
|
||||
explicit Matrix(const Quaternion<Scalar>& q);
|
||||
Matrix& operator=(const Quaternion<Scalar>& q);
|
||||
explicit Matrix(const AngleAxis<Scalar>& aa);
|
||||
Matrix& operator=(const AngleAxis<Scalar>& aa);
|
||||
};
|
||||
|
||||
/** \defgroup matrixtypedefs Global matrix typedefs
|
||||
|
||||
@ -325,7 +325,7 @@ template<typename Derived> class MatrixBase
|
||||
typename OtherDerived::Eval solveTriangular(const MatrixBase<OtherDerived>& other) const;
|
||||
|
||||
template<typename OtherDerived>
|
||||
void solveTriangularInPlace(MatrixBase<OtherDerived>* p_other) const;
|
||||
void solveTriangularInPlace(MatrixBase<OtherDerived>& other) const;
|
||||
|
||||
|
||||
template<typename OtherDerived>
|
||||
|
||||
@ -29,19 +29,19 @@ template<typename XprType> struct ei_is_part { enum {value=false}; };
|
||||
template<typename XprType, unsigned int Mode> struct ei_is_part<Part<XprType,Mode> > { enum {value=true}; };
|
||||
|
||||
template<typename Lhs, typename Rhs,
|
||||
int TriangularPart = ei_is_part<Lhs>::value ? -1 // this is to solve ambiguous specializations
|
||||
: (int(Lhs::Flags) & LowerTriangularBit)
|
||||
int TriangularPart = (int(Lhs::Flags) & LowerTriangularBit)
|
||||
? Lower
|
||||
: (int(Lhs::Flags) & UpperTriangularBit)
|
||||
? Upper
|
||||
: -1,
|
||||
int StorageOrder = int(Lhs::Flags) & RowMajorBit ? RowMajor : ColMajor
|
||||
int StorageOrder = ei_is_part<Lhs>::value ? -1 // this is to solve ambiguous specializations
|
||||
: int(Lhs::Flags) & RowMajorBit ? RowMajor : ColMajor
|
||||
>
|
||||
struct ei_solve_triangular_selector;
|
||||
|
||||
// transform a Part xpr to a Flagged xpr
|
||||
template<typename Lhs, unsigned int LhsMode, typename Rhs, int TriangularPart, int StorageOrder>
|
||||
struct ei_solve_triangular_selector<Part<Lhs,LhsMode>,Rhs,TriangularPart,StorageOrder>
|
||||
template<typename Lhs, unsigned int LhsMode, typename Rhs, int UpLo, int StorageOrder>
|
||||
struct ei_solve_triangular_selector<Part<Lhs,LhsMode>,Rhs,UpLo,StorageOrder>
|
||||
{
|
||||
static void run(const Part<Lhs,LhsMode>& lhs, Rhs& other)
|
||||
{
|
||||
@ -50,88 +50,129 @@ struct ei_solve_triangular_selector<Part<Lhs,LhsMode>,Rhs,TriangularPart,Storage
|
||||
};
|
||||
|
||||
// forward substitution, row-major
|
||||
template<typename Lhs, typename Rhs>
|
||||
struct ei_solve_triangular_selector<Lhs,Rhs,Lower,RowMajor>
|
||||
{
|
||||
typedef typename Rhs::Scalar Scalar;
|
||||
static void run(const Lhs& lhs, Rhs& other)
|
||||
{
|
||||
for(int c=0 ; c<other.cols() ; ++c)
|
||||
{
|
||||
if(!(Lhs::Flags & UnitDiagBit))
|
||||
other.coeffRef(0,c) = other.coeff(0,c)/lhs.coeff(0, 0);
|
||||
for(int i=1; i<lhs.rows(); ++i)
|
||||
{
|
||||
Scalar tmp = other.coeff(i,c) - ((lhs.row(i).start(i)) * other.col(c).start(i)).coeff(0,0);
|
||||
if (Lhs::Flags & UnitDiagBit)
|
||||
other.coeffRef(i,c) = tmp;
|
||||
else
|
||||
other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// backward substitution, row-major
|
||||
template<typename Lhs, typename Rhs>
|
||||
struct ei_solve_triangular_selector<Lhs,Rhs,Upper,RowMajor>
|
||||
template<typename Lhs, typename Rhs, int UpLo>
|
||||
struct ei_solve_triangular_selector<Lhs,Rhs,UpLo,RowMajor>
|
||||
{
|
||||
typedef typename Rhs::Scalar Scalar;
|
||||
static void run(const Lhs& lhs, Rhs& other)
|
||||
{
|
||||
const bool IsLower = (UpLo==Lower);
|
||||
const int size = lhs.cols();
|
||||
/* We perform the inverse product per block of 4 rows such that we perfectly match
|
||||
* our optimized matrix * vector product. blockyStart represents the number of rows
|
||||
* we have process first using the non-block version.
|
||||
*/
|
||||
int blockyStart = (std::max(size-5,0)/4)*4;
|
||||
if (IsLower)
|
||||
blockyStart = size - blockyStart;
|
||||
else
|
||||
blockyStart -= 1;
|
||||
for(int c=0 ; c<other.cols() ; ++c)
|
||||
{
|
||||
// process first rows using the non block version
|
||||
if(!(Lhs::Flags & UnitDiagBit))
|
||||
other.coeffRef(size-1,c) = other.coeff(size-1, c)/lhs.coeff(size-1, size-1);
|
||||
for(int i=size-2 ; i>=0 ; --i)
|
||||
if (IsLower)
|
||||
other.coeffRef(0,c) = other.coeff(0,c)/lhs.coeff(0, 0);
|
||||
else
|
||||
other.coeffRef(size-1,c) = other.coeff(size-1, c)/lhs.coeff(size-1, size-1);
|
||||
for(int i=(IsLower ? 1 : size-2); IsLower ? i<blockyStart : i>blockyStart; i += (IsLower ? 1 : -1) )
|
||||
{
|
||||
Scalar tmp = other.coeff(i,c)
|
||||
- ((lhs.row(i).end(size-i-1)) * other.col(c).end(size-i-1)).coeff(0,0);
|
||||
- (IsLower ? ((lhs.row(i).start(i)) * other.col(c).start(i)).coeff(0,0)
|
||||
: ((lhs.row(i).end(size-i-1)) * other.col(c).end(size-i-1)).coeff(0,0));
|
||||
if (Lhs::Flags & UnitDiagBit)
|
||||
other.coeffRef(i,c) = tmp;
|
||||
else
|
||||
other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
|
||||
}
|
||||
|
||||
// now let process the remaining rows 4 at once
|
||||
for(int i=blockyStart; IsLower ? i<size : i>0; )
|
||||
{
|
||||
int startBlock = i;
|
||||
int endBlock = startBlock + (IsLower ? 4 : -4);
|
||||
|
||||
/* Process the i cols times 4 rows block, and keep the result in a temporary vector */
|
||||
Matrix<Scalar,4,1> btmp;
|
||||
if (IsLower)
|
||||
btmp = lhs.block(startBlock,0,4,i) * other.col(c).start(i);
|
||||
else
|
||||
btmp = lhs.block(i-3,i+1,4,size-1-i) * other.col(c).end(size-1-i);
|
||||
|
||||
/* Let's process the 4x4 sub-matrix as usual.
|
||||
* btmp stores the diagonal coefficients used to update the remaining part of the result.
|
||||
*/
|
||||
{
|
||||
Scalar tmp = other.coeff(startBlock,c)-btmp.coeff(IsLower?0:3);
|
||||
if (Lhs::Flags & UnitDiagBit)
|
||||
other.coeffRef(i,c) = tmp;
|
||||
else
|
||||
other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
|
||||
}
|
||||
|
||||
i += IsLower ? 1 : -1;
|
||||
for (;IsLower ? i<endBlock : i>endBlock; i += IsLower ? 1 : -1)
|
||||
{
|
||||
int remainingSize = IsLower ? i-startBlock : startBlock-i;
|
||||
Scalar tmp = other.coeff(i,c)
|
||||
- btmp.coeff(IsLower ? remainingSize : 3-remainingSize)
|
||||
- ( lhs.row(i).block(IsLower ? startBlock : i+1, remainingSize)
|
||||
* other.col(c).block(IsLower ? startBlock : i+1, remainingSize)).coeff(0,0);
|
||||
|
||||
if (Lhs::Flags & UnitDiagBit)
|
||||
other.coeffRef(i,c) = tmp;
|
||||
else
|
||||
other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// forward substitution, col-major
|
||||
// FIXME the Lower and Upper specialization could be merged using a small helper class
|
||||
// performing reflexions on the coordinates...
|
||||
template<typename Lhs, typename Rhs>
|
||||
struct ei_solve_triangular_selector<Lhs,Rhs,Lower,ColMajor>
|
||||
// Implements the following configurations:
|
||||
// - inv(Lower, ColMajor) * Column vector
|
||||
// - inv(Lower,UnitDiag,ColMajor) * Column vector
|
||||
// - inv(Upper, ColMajor) * Column vector
|
||||
// - inv(Upper,UnitDiag,ColMajor) * Column vector
|
||||
template<typename Lhs, typename Rhs, int UpLo>
|
||||
struct ei_solve_triangular_selector<Lhs,Rhs,UpLo,ColMajor>
|
||||
{
|
||||
typedef typename Rhs::Scalar Scalar;
|
||||
typedef typename ei_packet_traits<Scalar>::type Packet;
|
||||
enum {PacketSize = ei_packet_traits<Scalar>::size};
|
||||
enum { PacketSize = ei_packet_traits<Scalar>::size };
|
||||
|
||||
static void run(const Lhs& lhs, Rhs& other)
|
||||
{
|
||||
static const bool IsLower = (UpLo==Lower);
|
||||
const int size = lhs.cols();
|
||||
for(int c=0 ; c<other.cols() ; ++c)
|
||||
{
|
||||
/* let's perform the inverse product per block of 4 columns such that we perfectly match
|
||||
* our optimized matrix * vector product.
|
||||
* our optimized matrix * vector product. blockyEnd represents the number of rows
|
||||
* we can process using the block version.
|
||||
*/
|
||||
int blockyEnd = (std::max(size-5,0)/4)*4;
|
||||
for(int i=0; i<blockyEnd;)
|
||||
if (!IsLower)
|
||||
blockyEnd = size-1 - blockyEnd;
|
||||
for(int i=IsLower ? 0 : size-1; IsLower ? i<blockyEnd : i>blockyEnd;)
|
||||
{
|
||||
/* Let's process the 4x4 sub-matrix as usual.
|
||||
* btmp stores the diagonal coefficients used to update the remaining part of the result.
|
||||
*/
|
||||
int startBlock = i;
|
||||
int endBlock = startBlock+4;
|
||||
int endBlock = startBlock + (IsLower ? 4 : -4);
|
||||
Matrix<Scalar,4,1> btmp;
|
||||
for (;i<endBlock;++i)
|
||||
for (;IsLower ? i<endBlock : i>endBlock;
|
||||
i += IsLower ? 1 : -1)
|
||||
{
|
||||
if(!(Lhs::Flags & UnitDiagBit))
|
||||
other.coeffRef(i,c) /= lhs.coeff(i,i);
|
||||
int remainingSize = endBlock-i-1;
|
||||
int remainingSize = IsLower ? endBlock-i-1 : i-endBlock-1;
|
||||
if (remainingSize>0)
|
||||
other.col(c).block(i+1,remainingSize) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, i+1, i, remainingSize, 1);
|
||||
btmp.coeffRef(i-startBlock) = -other.coeffRef(i,c);
|
||||
other.col(c).block((IsLower ? i : endBlock) + 1, remainingSize) -=
|
||||
other.coeffRef(i,c)
|
||||
* Block<Lhs,Dynamic,1>(lhs, (IsLower ? i : endBlock) + 1, i, remainingSize, 1);
|
||||
btmp.coeffRef(IsLower ? i-startBlock : remainingSize) = -other.coeffRef(i,c);
|
||||
}
|
||||
|
||||
/* Now we can efficiently update the remaining part of the result as a matrix * vector product.
|
||||
@ -143,13 +184,15 @@ struct ei_solve_triangular_selector<Lhs,Rhs,Lower,ColMajor>
|
||||
// FIXME this is cool but what about conjugate/adjoint expressions ? do we want to evaluate them ?
|
||||
// this is a more general problem though.
|
||||
ei_cache_friendly_product_colmajor_times_vector(
|
||||
size-endBlock, &(lhs.const_cast_derived().coeffRef(endBlock,startBlock)), lhs.stride(),
|
||||
btmp, &(other.coeffRef(endBlock,c)));
|
||||
IsLower ? size-endBlock : endBlock+1,
|
||||
&(lhs.const_cast_derived().coeffRef(IsLower ? endBlock : 0, IsLower ? startBlock : endBlock+1)),
|
||||
lhs.stride(),
|
||||
btmp, &(other.coeffRef(IsLower ? endBlock : 0, c)));
|
||||
}
|
||||
|
||||
/* Now we have to process the remaining part as usual */
|
||||
int i;
|
||||
for(i=blockyEnd; i<size-1; ++i)
|
||||
for(i=blockyEnd; IsLower ? i<size-1 : i>0; i += (IsLower ? 1 : -1) )
|
||||
{
|
||||
if(!(Lhs::Flags & UnitDiagBit))
|
||||
other.coeffRef(i,c) /= lhs.coeff(i,i);
|
||||
@ -157,7 +200,10 @@ struct ei_solve_triangular_selector<Lhs,Rhs,Lower,ColMajor>
|
||||
/* NOTE we cannot use lhs.col(i).end(size-i-1) because Part::coeffRef gets called by .col() to
|
||||
* get the address of the start of the row
|
||||
*/
|
||||
other.col(c).end(size-i-1) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, i+1,i, size-i-1,1);
|
||||
if(IsLower)
|
||||
other.col(c).end(size-i-1) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, i+1,i, size-i-1,1);
|
||||
else
|
||||
other.col(c).start(i) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, 0,i, i, 1);
|
||||
}
|
||||
if(!(Lhs::Flags & UnitDiagBit))
|
||||
other.coeffRef(i,c) /= lhs.coeff(i,i);
|
||||
@ -165,68 +211,20 @@ struct ei_solve_triangular_selector<Lhs,Rhs,Lower,ColMajor>
|
||||
}
|
||||
};
|
||||
|
||||
// backward substitution, col-major
|
||||
// see the previous specialization for details on the algorithm
|
||||
template<typename Lhs, typename Rhs>
|
||||
struct ei_solve_triangular_selector<Lhs,Rhs,Upper,ColMajor>
|
||||
{
|
||||
typedef typename Rhs::Scalar Scalar;
|
||||
static void run(const Lhs& lhs, Rhs& other)
|
||||
{
|
||||
const int size = lhs.cols();
|
||||
for(int c=0 ; c<other.cols() ; ++c)
|
||||
{
|
||||
int blockyEnd = size-1 - (std::max(size-5,0)/4)*4;
|
||||
for(int i=size-1; i>blockyEnd;)
|
||||
{
|
||||
int startBlock = i;
|
||||
int endBlock = startBlock-4;
|
||||
Matrix<Scalar,4,1> btmp;
|
||||
/* Let's process the 4x4 sub-matrix as usual.
|
||||
* btmp stores the diagonal coefficients used to update the remaining part of the result.
|
||||
*/
|
||||
for (; i>endBlock; --i)
|
||||
{
|
||||
if(!(Lhs::Flags & UnitDiagBit))
|
||||
other.coeffRef(i,c) /= lhs.coeff(i,i);
|
||||
int remainingSize = i-endBlock-1;
|
||||
if (remainingSize>0)
|
||||
other.col(c).block(endBlock+1,remainingSize) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, endBlock+1, i, remainingSize, 1);
|
||||
btmp.coeffRef(remainingSize) = -other.coeffRef(i,c);
|
||||
}
|
||||
|
||||
ei_cache_friendly_product_colmajor_times_vector(
|
||||
endBlock+1, &(lhs.const_cast_derived().coeffRef(0,endBlock+1)), lhs.stride(),
|
||||
btmp, &(other.coeffRef(0,c)));
|
||||
}
|
||||
|
||||
for(int i=blockyEnd; i>0; --i)
|
||||
{
|
||||
if(!(Lhs::Flags & UnitDiagBit))
|
||||
other.coeffRef(i,c) /= lhs.coeff(i,i);
|
||||
other.col(c).start(i) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, 0,i, i, 1);
|
||||
}
|
||||
if(!(Lhs::Flags & UnitDiagBit))
|
||||
other.coeffRef(0,c) /= lhs.coeff(0,0);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/** "in-place" version of MatrixBase::solveTriangular() where the result is written in \a other
|
||||
*
|
||||
* See MatrixBase:solveTriangular() for the details.
|
||||
*/
|
||||
template<typename Derived>
|
||||
template<typename OtherDerived>
|
||||
void MatrixBase<Derived>::solveTriangularInPlace(MatrixBase<OtherDerived>* p_other) const
|
||||
void MatrixBase<Derived>::solveTriangularInPlace(MatrixBase<OtherDerived>& other) const
|
||||
{
|
||||
ei_assert(p_other!=0);
|
||||
ei_assert(derived().cols() == derived().rows());
|
||||
ei_assert(derived().cols() == p_other->rows());
|
||||
ei_assert(derived().cols() == other.rows());
|
||||
ei_assert(!(Flags & ZeroDiagBit));
|
||||
ei_assert(Flags & (UpperTriangularBit|LowerTriangularBit));
|
||||
|
||||
ei_solve_triangular_selector<Derived, OtherDerived>::run(derived(), p_other->derived());
|
||||
ei_solve_triangular_selector<Derived, OtherDerived>::run(derived(), other.derived());
|
||||
}
|
||||
|
||||
/** \returns the product of the inverse of \c *this with \a other, \a *this being triangular.
|
||||
@ -265,7 +263,7 @@ template<typename OtherDerived>
|
||||
typename OtherDerived::Eval MatrixBase<Derived>::solveTriangular(const MatrixBase<OtherDerived>& other) const
|
||||
{
|
||||
typename OtherDerived::Eval res(other);
|
||||
solveTriangularInPlace(&res);
|
||||
solveTriangularInPlace(res);
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
@ -59,6 +59,7 @@
|
||||
you_mixed_vectors_of_different_sizes,
|
||||
you_mixed_matrices_of_different_sizes,
|
||||
this_method_is_only_for_vectors_of_a_specific_size,
|
||||
this_method_is_only_for_matrices_of_a_specific_size,
|
||||
you_did_a_programming_error,
|
||||
you_called_a_fixed_size_method_on_a_dynamic_size_matrix_or_vector,
|
||||
unaligned_load_and_store_operations_unimplemented_on_AltiVec,
|
||||
@ -96,6 +97,11 @@
|
||||
EIGEN_STATIC_ASSERT(TYPE::IsVectorAtCompileTime && TYPE::SizeAtCompileTime==SIZE, \
|
||||
this_method_is_only_for_vectors_of_a_specific_size)
|
||||
|
||||
// static assertion failing if the type \a TYPE is not a vector type of the given size
|
||||
#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(TYPE, ROWS, COLS) \
|
||||
EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime==ROWS && TYPE::ColsAtCompileTime==COLS, \
|
||||
this_method_is_only_for_matrices_of_a_specific_size)
|
||||
|
||||
// static assertion failing if the two vector expression types are not compatible (same fixed-size or dynamic size)
|
||||
#define EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TYPE0,TYPE1) \
|
||||
EIGEN_STATIC_ASSERT( \
|
||||
|
||||
@ -82,27 +82,32 @@ public:
|
||||
const Vector3& axis() const { return m_axis; }
|
||||
Vector3& axis() { return m_axis; }
|
||||
|
||||
/** Automatic conversion to a 3x3 rotation matrix.
|
||||
* \sa toRotationMatrix() */
|
||||
operator Matrix3 () const { return toRotationMatrix(); }
|
||||
|
||||
/** Concatenates two rotations */
|
||||
inline QuaternionType operator* (const AngleAxis& other) const
|
||||
{ return QuaternionType(*this) * QuaternionType(other); }
|
||||
|
||||
|
||||
/** Concatenates two rotations */
|
||||
inline QuaternionType operator* (const QuaternionType& other) const
|
||||
{ return QuaternionType(*this) * other; }
|
||||
|
||||
/** Concatenates two rotations */
|
||||
friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
|
||||
{ return a * QuaternionType(b); }
|
||||
|
||||
/** Concatenates two rotations */
|
||||
inline typename ProductReturnType<Matrix3,Matrix3>::Type
|
||||
operator* (const Matrix3& other) const
|
||||
{ return toRotationMatrix() * other; }
|
||||
|
||||
/** Concatenates two rotations */
|
||||
inline friend typename ProductReturnType<Matrix3,Matrix3>::Type
|
||||
operator* (const Matrix3& a, const AngleAxis& b)
|
||||
{ return a * b.toRotationMatrix(); }
|
||||
|
||||
/** \Returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
|
||||
AngleAxis inverse() const
|
||||
{ return AngleAxis(-m_angle, m_axis); }
|
||||
|
||||
AngleAxis& operator=(const QuaternionType& q);
|
||||
template<typename Derived>
|
||||
AngleAxis& operator=(const MatrixBase<Derived>& m);
|
||||
@ -179,4 +184,29 @@ AngleAxis<Scalar>::toRotationMatrix(void) const
|
||||
return res;
|
||||
}
|
||||
|
||||
/** \geometry_module
|
||||
*
|
||||
* Constructs a 3x3 rotation matrix from the angle-axis \a aa
|
||||
*
|
||||
* \sa Matrix(const Quaternion&)
|
||||
*/
|
||||
template<typename _Scalar, int _Rows, int _Cols, int _MaxRows, int _MaxCols, unsigned int _Flags>
|
||||
Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>::Matrix(const AngleAxis<Scalar>& aa)
|
||||
{
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,3,3);
|
||||
*this = aa.toRotationMatrix();
|
||||
}
|
||||
|
||||
/** \geometry_module
|
||||
*
|
||||
* Set a 3x3 rotation matrix from the angle-axis \a aa
|
||||
*/
|
||||
template<typename _Scalar, int _Rows, int _Cols, int _MaxRows, int _MaxCols, unsigned int _Flags>
|
||||
Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>&
|
||||
Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>::operator=(const AngleAxis<Scalar>& aa)
|
||||
{
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,3,3);
|
||||
return *this = aa.toRotationMatrix();
|
||||
}
|
||||
|
||||
#endif // EIGEN_ANGLEAXIS_H
|
||||
|
||||
@ -118,6 +118,7 @@ public:
|
||||
|
||||
/** Constructs and initializes a quaternion from the angle-axis \a aa */
|
||||
explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
|
||||
|
||||
/** Constructs and initializes a quaternion from either:
|
||||
* - a rotation matrix expression,
|
||||
* - a 4D vector expression representing quaternion coefficients.
|
||||
@ -131,9 +132,6 @@ public:
|
||||
template<typename Derived>
|
||||
Quaternion& operator=(const MatrixBase<Derived>& m);
|
||||
|
||||
/** Automatic conversion to a rotation matrix. */
|
||||
operator Matrix3 () const { return toRotationMatrix(); }
|
||||
|
||||
/** \returns a quaternion representing an identity rotation
|
||||
* \sa MatrixBase::Identity()
|
||||
*/
|
||||
@ -426,4 +424,29 @@ struct ei_quaternion_assign_impl<Other,4,1>
|
||||
}
|
||||
};
|
||||
|
||||
/** \geometry_module
|
||||
*
|
||||
* Constructs a 3x3 rotation matrix from the quaternion \a q
|
||||
*
|
||||
* \sa Matrix(const AngleAxis&)
|
||||
*/
|
||||
template<typename _Scalar, int _Rows, int _Cols, int _MaxRows, int _MaxCols, unsigned int _Flags>
|
||||
Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>::Matrix(const Quaternion<Scalar>& q)
|
||||
{
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,3,3);
|
||||
*this = q.toRotationMatrix();
|
||||
}
|
||||
|
||||
/** \geometry_module
|
||||
*
|
||||
* Set a 3x3 rotation matrix from the quaternion \a q
|
||||
*/
|
||||
template<typename _Scalar, int _Rows, int _Cols, int _MaxRows, int _MaxCols, unsigned int _Flags>
|
||||
Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>&
|
||||
Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>::operator=(const Quaternion<Scalar>& q)
|
||||
{
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,3,3);
|
||||
return *this = q.toRotationMatrix();
|
||||
}
|
||||
|
||||
#endif // EIGEN_QUATERNION_H
|
||||
|
||||
@ -126,6 +126,7 @@ public:
|
||||
enum { Dim = 2 };
|
||||
/** the scalar type of the coefficients */
|
||||
typedef _Scalar Scalar;
|
||||
typedef Matrix<Scalar,2,1> Vector2;
|
||||
typedef Matrix<Scalar,2,2> Matrix2;
|
||||
|
||||
protected:
|
||||
@ -136,14 +137,34 @@ public:
|
||||
|
||||
/** Construct a 2D counter clock wise rotation from the angle \a a in radian. */
|
||||
inline Rotation2D(Scalar a) : m_angle(a) {}
|
||||
inline operator Scalar& () { return m_angle; }
|
||||
inline operator Scalar () const { return m_angle; }
|
||||
|
||||
/** \Returns the rotation angle */
|
||||
inline Scalar angle() const { return m_angle; }
|
||||
|
||||
/** \Returns a read-write reference to the rotation angle */
|
||||
inline Scalar& angle() { return m_angle; }
|
||||
|
||||
/** Automatic convertion to a 2D rotation matrix.
|
||||
* \sa toRotationMatrix()
|
||||
*/
|
||||
inline operator Matrix2() const { return toRotationMatrix(); }
|
||||
|
||||
/** \Returns the inverse rotation */
|
||||
inline Rotation2D inverse() const { return -m_angle; }
|
||||
|
||||
/** Concatenates two rotations */
|
||||
inline Rotation2D operator*(const Rotation2D& other) const
|
||||
{ return m_angle + other.m_angle; }
|
||||
|
||||
/** Concatenates two rotations */
|
||||
inline Rotation2D& operator*=(const Rotation2D& other)
|
||||
{ return m_angle += other.m_angle; }
|
||||
|
||||
/** Applies the rotation to a 2D vector */
|
||||
template<typename Derived>
|
||||
Vector2 operator* (const MatrixBase<Derived>& vec) const
|
||||
{ return toRotationMatrix() * vec; }
|
||||
|
||||
template<typename Derived>
|
||||
Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
|
||||
Matrix2 toRotationMatrix(void) const;
|
||||
|
||||
@ -399,7 +399,7 @@ void LU<MatrixType>::computeKernel(Matrix<typename MatrixType::Scalar,
|
||||
|
||||
m_lu.corner(TopLeft, m_rank, m_rank)
|
||||
.template marked<Upper>()
|
||||
.solveTriangularInPlace(&y);
|
||||
.solveTriangularInPlace(y);
|
||||
|
||||
for(int i = 0; i < m_rank; i++)
|
||||
result->row(m_q.coeff(i)) = y.row(i);
|
||||
@ -451,7 +451,7 @@ bool LU<MatrixType>::solve(
|
||||
l.setZero();
|
||||
l.corner(Eigen::TopLeft,rows,smalldim)
|
||||
= m_lu.corner(Eigen::TopLeft,rows,smalldim);
|
||||
l.template marked<UnitLower>().solveTriangularInPlace(&c);
|
||||
l.template marked<UnitLower>().solveTriangularInPlace(c);
|
||||
|
||||
// Step 3
|
||||
if(!isSurjective())
|
||||
@ -468,7 +468,7 @@ bool LU<MatrixType>::solve(
|
||||
d(c.corner(TopLeft, m_rank, c.cols()));
|
||||
m_lu.corner(TopLeft, m_rank, m_rank)
|
||||
.template marked<Upper>()
|
||||
.solveTriangularInPlace(&d);
|
||||
.solveTriangularInPlace(d);
|
||||
|
||||
// Step 4
|
||||
result->resize(m_lu.cols(), b.cols());
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user