void gemv(
matrix_expression<MatrA> const &A,
vector_expression<VectorX> const &x,
vector_expression<VectorY> &y,
typename VectorY::value_type alpha,
boost::mpl::true_
){
std::size_t m = A().size1();
std::size_t n = A().size2();
SIZE_CHECK(x().size() == A().size2());
SIZE_CHECK(y().size() == A().size1());
CBLAS_ORDER const stor_ord= (CBLAS_ORDER)storage_order<typename MatrA::orientation>::value;
gemv(stor_ord, CblasNoTrans, (int)m, (int)n, alpha,
traits::storage(A),
traits::leading_dimension(A),
traits::storage(x),
traits::stride(x),
typename VectorY::value_type(1),
traits::storage(y),
traits::stride(y)
);
}
void gemv(
matrix_expression<MatA, cpu_tag> const &A,
vector_expression<VectorX, cpu_tag> const &x,
vector_expression<VectorY, cpu_tag> &y,
typename VectorY::value_type alpha,
boost::mpl::true_
){
std::size_t m = A().size1();
std::size_t n = A().size2();
SIZE_CHECK(x().size() == A().size2());
SIZE_CHECK(y().size() == A().size1());
CBLAS_ORDER const stor_ord= (CBLAS_ORDER)storage_order<typename MatA::orientation>::value;
auto storageA = A().raw_storage();
auto storagex = x().raw_storage();
auto storagey = y().raw_storage();
gemv(stor_ord, CblasNoTrans, (int)m, (int)n, alpha,
storageA.values,
storageA.leading_dimension,
storagex.values,
storagex.stride,
typename VectorY::value_type(1),
storagey.values,
storagey.stride
);
}
void trsm(
matrix_expression<MatA, cpu_tag> const &A,
matrix_expression<MatB, cpu_tag> &B,
boost::mpl::true_
){
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size1() == B().size1());
//orientation is defined by the second argument
CBLAS_ORDER const storOrd = (CBLAS_ORDER)storage_order<typename MatB::orientation>::value;
//if orientations do not match, wecan interpret this as transposing A
bool transposeA = !std::is_same<typename MatA::orientation,typename MatB::orientation>::value;
CBLAS_DIAG cblasUnit = unit?CblasUnit:CblasNonUnit;
CBLAS_UPLO cblasUplo = (upper != transposeA)?CblasUpper:CblasLower;
CBLAS_TRANSPOSE transA = transposeA?CblasTrans:CblasNoTrans;
int m = B().size1();
int nrhs = B().size2();
auto storageA = A().raw_storage();
auto storageB = B().raw_storage();
trsm(storOrd, cblasUplo, transA, CblasLeft,cblasUnit, m, nrhs,
storageA.values,
storageA.leading_dimension,
storageB.values,
storageB.leading_dimension
);
}
void assign(
vector_expression<V>& v, vector_expression<E> const& e,
packed_random_access_iterator_tag, packed_random_access_iterator_tag
) {
SIZE_CHECK(v().size() == e().size());
typedef typename E::const_iterator EIterator;
EIterator eiter = e.begin();
EIterator eend = e.end();
//special case:
//right hand side is completely 0
if(eiter == eend) {
v().clear();
return;
}
EIterator viter = v.begin();
EIterator vend = v.end();
//check for compatible layout
SIZE_CHECK(vend-viter);//empty ranges can't be compatible
//check whether the right hand side range is included in the left hand side range
SIZE_CHECK(viter.index() <= eiter.index());
SIZE_CHECK(viter.index()+(vend-viter) >= eiter.index()+(eend-eiter));
//copy contents of right-hand side
viter += eiter.index()-viter.index();
for(; eiter != eend; ++eiter,++viter) {
*viter= *eiter;
}
}
void gemm(
matrix_expression<MatrA> const &matA,
matrix_expression<MatrB> const &matB,
matrix_expression<MatrC>& matC,
typename MatrC::value_type alpha,
boost::mpl::true_
) {
SIZE_CHECK(matA().size1() == matC().size1());
SIZE_CHECK(matB().size2() == matC().size2());
SIZE_CHECK(matA().size2()== matB().size1());
CBLAS_TRANSPOSE transA = traits::same_orientation(matA,matC)?CblasNoTrans:CblasTrans;
CBLAS_TRANSPOSE transB = traits::same_orientation(matB,matC)?CblasNoTrans:CblasTrans;
std::size_t m = matC().size1();
std::size_t n = matC().size2();
std::size_t k = matA().size2();
CBLAS_ORDER stor_ord = (CBLAS_ORDER) storage_order<typename MatrC::orientation >::value;
gemm(stor_ord, transA, transB, (int)m, (int)n, (int)k, alpha,
traits::storage(matA()),
traits::leading_dimension(matA()),
traits::storage(matB()),
traits::leading_dimension(matB()),
typename MatrC::value_type(1),
traits::storage(matC()),
traits::leading_dimension(matC())
);
}
void trsm_impl(
matrix_expression<MatA, cpu_tag> const& A, matrix_expression<MatB, cpu_tag>& B,
boost::mpl::true_, column_major
) {
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size2() == B().size1());
typedef typename MatA::value_type value_type;
std::size_t size1 = B().size1();
std::size_t size2 = B().size2();
for (std::size_t i = 0; i < size1; ++ i) {
std::size_t n = size1-i-1;
auto columnTriangular = column(A(),n);
if(!Unit){
RANGE_CHECK(A()(n, n) != value_type());//matrix is singular
row(B(),n) /= A()(n, n);
}
for (std::size_t l = 0; l < size2; ++ l) {
if (B()(n, l) != value_type/*zero*/()) {
auto columnMatrix = column(B(),l);
noalias(subrange(columnMatrix,0,n)) -= B()(n,l) * subrange(columnTriangular,0,n);
}
}
}
}
void trsm(
matrix_expression<TriangularA> const &A,
matrix_expression<MatB> &B,
boost::mpl::true_
){
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size1() == B().size1());
//orientation is defined by the second argument
CBLAS_ORDER const storOrd = (CBLAS_ORDER)storage_order<typename MatB::orientation>::value;
//if orientations do not match, wecan interpret this as transposing A
bool transposeA = !traits::same_orientation(A,B);
CBLAS_DIAG cblasUnit = unit?CblasUnit:CblasNonUnit;
CBLAS_UPLO cblasUplo = (upper != transposeA)?CblasUpper:CblasLower;
CBLAS_TRANSPOSE transA = transposeA?CblasTrans:CblasNoTrans;
int m = B().size1();
int nrhs = B().size2();
trsm(storOrd, cblasUplo, transA, CblasLeft,cblasUnit, m, nrhs,
traits::storage(A),
traits::leading_dimension(A),
traits::storage(B),
traits::leading_dimension(B)
);
}
void trmm(
matrix_expression<MatA> const& A,
matrix_expression<MatB>& B,
boost::mpl::true_
){
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size2() == B().size1());
std::size_t n = A().size1();
std::size_t m = B().size2();
CBLAS_DIAG cblasUnit = unit?CblasUnit:CblasNonUnit;
CBLAS_UPLO cblasUplo = upper?CblasUpper:CblasLower;
CBLAS_ORDER stor_ord= (CBLAS_ORDER)storage_order<typename MatA::orientation>::value;
CBLAS_TRANSPOSE trans=CblasNoTrans;
//special case: MatA and MatB do not have same storage order. in this case compute as
//AB->B^TA^T where transpose of B is done implicitely by exchanging storage order
CBLAS_ORDER stor_ordB= (CBLAS_ORDER)storage_order<typename MatB::orientation>::value;
if(stor_ord != stor_ordB){
trans = CblasTrans;
cblasUplo= upper?CblasLower:CblasUpper;
}
trmm(stor_ordB, CblasLeft, cblasUplo, trans, cblasUnit,
(int)n, int(m),
traits::storage(A),
traits::leading_dimension(A),
traits::storage(B),
traits::leading_dimension(B)
);
}
void trsv(
matrix_expression<MatA, cpu_tag> const &A,
vector_expression<V, cpu_tag> &b
){
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size1() == b().size());
bindings::trsv<Upper,Unit>(A,b,typename bindings::has_optimized_trsv<MatA, V>::type());
}
void tpmv(
matrix_expression<TriangularA> const &A,
vector_expression<VecB>& b
){
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size1() == b().size());
bindings::tpmv(A,b,typename bindings::has_optimized_tpmv<TriangularA, VecB>::type());
}
void trmv(
matrix_expression<TriangularA> const& A,
vector_expression<V> & b,
boost::mpl::false_//unoptimized
){
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size2() == b().size());
trmv_impl<Unit>(A, b, boost::mpl::bool_<Upper>(), typename TriangularA::orientation());
}
void trmm(
matrix_expression<TriangularA> const &A,
matrix_expression<MatB>& B
){
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size1() == B().size1());
bindings::trmm<Upper,Unit>(A,B,typename bindings::has_optimized_trmm<TriangularA, MatB>::type());
}
void gemv(
matrix_expression<M> const& A,
vector_expression<V> const& x,
vector_expression<ResultV>& result,
typename ResultV::value_type alpha,
boost::mpl::false_
) {
SIZE_CHECK(A().size1()==result().size());
SIZE_CHECK(A().size2()==x().size());
typedef typename M::orientation orientation;
gemv_impl(A, x, result, alpha, orientation());
}
void gemv(
matrix_expression<E1, cpu_tag> const& e1,
vector_expression<E2, cpu_tag> const& e2,
vector_expression<M, cpu_tag>& m,
typename M::value_type alpha
) {
SIZE_CHECK(m().size() == e1().size1());
SIZE_CHECK(e1().size2() == e2().size());
bindings::gemv(
e1, e2, m,alpha,
typename bindings::has_optimized_gemv<M,E1,E2>::type()
);
}
void gemm(
matrix_expression<E1> const& e1,
matrix_expression<E2> const& e2,
matrix_expression<M>& m,
typename M::value_type alpha
) {
SIZE_CHECK(m().size1() == e1().size1());
SIZE_CHECK(m().size2() == e2().size2());
SIZE_CHECK(e1().size2() == e2().size1());
bindings::gemm(
e1, e2, m,alpha,
typename bindings::has_optimized_gemm<M,E1,E2>::type()
);
}
void trmm(
matrix_expression<MatA, cpu_tag> const &A,
matrix_expression<MatB, cpu_tag>& B,
boost::mpl::false_
) {
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size1() == B().size1());
std::size_t numCols=B().size2();
for(std::size_t j = 0; j != numCols; ++j){
auto col = column(B,j);
kernels::trmv<Upper,Unit>(A,col);
}
}
// Element access
const_reference operator()(size_type i) const {
SIZE_CHECK(i < m_size);
std::size_t pos = lower_bound(i);
if (pos == nnz() || m_indices[pos] != i)
return m_zero;
return m_values [pos];
}
void assign(
vector_expression<V>& v,
vector_expression<E> const& e,
F f,
dense_random_access_iterator_tag, packed_random_access_iterator_tag
) {
SIZE_CHECK(v().size() == e().size());
typedef typename E::const_iterator EIterator;
typedef typename V::const_iterator VIterator;
typedef typename V::scalar_type scalar_type;
EIterator eiter = e().begin();
EIterator eend = e().end();
VIterator viter = v().begin();
VIterator vend = v().end();
//right hand side hasnonzero elements
if(eiter != eend) {
//apply f to the first elements for which the right hand side is 0, unless f is the identity
for(; viter.index() != eiter.index() &&!F::right_zero_identity; ++viter) {
f(*viter,scalar_type/*zero*/());
}
//copy contents of right-hand side
for(; eiter != eend; ++eiter,++viter) {
f(*viter,*eiter);
}
}
//apply f to the last elements for which the right hand side is 0, unless f is the identity
for(; viter!= vend &&!F::right_zero_identity; ++viter) {
*viter= scalar_type/*zero*/();
}
}
void assign(
vector_expression<V>& v, vector_expression<E> const& e,
dense_random_access_iterator_tag, packed_random_access_iterator_tag
) {
SIZE_CHECK(v().size() == e().size());
typedef typename E::const_iterator EIterator;
typedef typename V::scalar_type scalar_type;
EIterator eiter = e.begin();
EIterator eend = e.end();
//special case:
//right hand side is completely 0
if(eiter == eend) {
v().clear();
return;
}
EIterator viter = v.begin();
EIterator vend = v.end();
//set the first elements to zero
for(; viter.index() != eiter.index(); ++viter) {
*viter= scalar_type/*zero*/();
}
//copy contents of right-hand side
for(; eiter != eend; ++eiter,++viter) {
*viter= *eiter;
}
for(; viter!= vend; ++viter) {
*viter= scalar_type/*zero*/();
}
}
void assign(vector_expression<V>& v, const vector_expression<E> &e) {
SIZE_CHECK(v().size() == e().size());
typedef typename V::const_iterator::iterator_category CategoryV;
typedef typename E::const_iterator::iterator_category CategoryE;
typedef F<typename V::iterator::reference, typename E::value_type> functor_type;
assign(v(), e(), functor_type(), CategoryV(),CategoryE());
}
uint32_t pstorage_load(uint8_t * p_dest,
pstorage_handle_t * p_src,
pstorage_size_t size,
pstorage_size_t offset)
{
VERIFY_MODULE_INITIALIZED();
NULL_PARAM_CHECK(p_src);
NULL_PARAM_CHECK(p_dest);
MODULE_ID_RANGE_CHECK (p_src);
BLOCK_ID_RANGE_CHECK(p_src);
SIZE_CHECK(p_src,size);
OFFSET_CHECK(p_src,offset,size);
// Verify word alignment.
if ((!is_word_aligned(p_dest)) || (!is_word_aligned((void *)(uint32_t)offset)))
{
return NRF_ERROR_INVALID_ADDR;
}
memcpy(p_dest, (((uint8_t *)p_src->block_id) + offset), size);
app_notify(p_src, p_dest, PSTORAGE_LOAD_OP_CODE, size, NRF_SUCCESS);
return NRF_SUCCESS;
}
uint32_t pstorage_store(pstorage_handle_t * p_dest,
uint8_t * p_src,
pstorage_size_t size,
pstorage_size_t offset)
{
VERIFY_MODULE_INITIALIZED();
NULL_PARAM_CHECK(p_src);
NULL_PARAM_CHECK(p_dest);
MODULE_ID_RANGE_CHECK(p_dest);
BLOCK_ID_RANGE_CHECK(p_dest);
SIZE_CHECK(p_dest, size);
OFFSET_CHECK(p_dest, offset, size);
// Verify word alignment.
if ((!is_word_aligned(p_src)) || (!is_word_aligned((void *)(uint32_t)offset)))
{
return NRF_ERROR_INVALID_ADDR;
}
uint32_t storage_addr = p_dest->block_id + offset;
uint32_t retval = ble_flash_block_write((uint32_t *)storage_addr,
(uint32_t *)p_src,
(size /sizeof(uint32_t)));
app_notify(p_dest, p_src, PSTORAGE_STORE_OP_CODE, size, retval);
return retval;
}
uint32_t pstorage_store(pstorage_handle_t * p_dest,
uint8_t * p_src,
pstorage_size_t size,
pstorage_size_t offset)
{
VERIFY_MODULE_INITIALIZED();
NULL_PARAM_CHECK(p_src);
NULL_PARAM_CHECK(p_dest);
MODULE_ID_RANGE_CHECK(p_dest);
BLOCK_ID_RANGE_CHECK(p_dest);
SIZE_CHECK(p_dest, size);
OFFSET_CHECK(p_dest, offset,size);
// Verify word alignment.
if ((!is_word_aligned(p_src)) || (!is_word_aligned((void *)(uint32_t)offset)))
{
return NRF_ERROR_INVALID_ADDR;
}
if ((!is_word_aligned((uint32_t *)p_dest->block_id)))
{
return NRF_ERROR_INVALID_ADDR;
}
return cmd_queue_enqueue(PSTORAGE_STORE_OP_CODE, p_dest, p_src, size, offset);
}
void assign(
vector_expression<V>& v, vector_expression<E> const& e,
dense_random_access_iterator_tag, dense_random_access_iterator_tag
) {
SIZE_CHECK(v().size() == e().size());
for(std::size_t i = 0; i != v().size(); ++i) {
v()(i)=e()(i);
}
}
void trmv(
matrix_expression<MatrA> const& A,
vector_expression<VectorX> &x,
boost::mpl::true_
){
SIZE_CHECK(x().size() == A().size2());
SIZE_CHECK(A().size2() == A().size1());
std::size_t n = A().size1();
CBLAS_DIAG cblasUnit = unit?CblasUnit:CblasNonUnit;
CBLAS_UPLO cblasUplo = upper?CblasUpper:CblasLower;
CBLAS_ORDER stor_ord= (CBLAS_ORDER)storage_order<typename MatrA::orientation>::value;
trmv(stor_ord, cblasUplo, CblasNoTrans, cblasUnit, (int)n,
traits::storage(A),
traits::leading_dimension(A),
traits::storage(x),
traits::stride(x)
);
}
inline int potrf(CBLAS_UPLO const uplo, matrix_container<SymmA>& a) {
CBLAS_ORDER const stor_ord=
(CBLAS_ORDER)storage_order<typename SymmA::orientation>::value;
std::size_t n = a().size1();
SIZE_CHECK(n == a().size2());
return potrf(stor_ord, uplo, (int)n,
traits::storage(a()),
traits::leading_dimension(a()));
}
void dot(
vector_expression<E1> const& v1,
vector_expression<E2> const& v2,
result_type& result
) {
SIZE_CHECK(v1().size()==v2().size());
return dot_impl(v1,v2,result,
typename E1::const_iterator::iterator_category(),
typename E2::const_iterator::iterator_category()
);
}
void trsm_impl(
matrix_expression<MatA, cpu_tag> const& A, matrix_expression<MatB, cpu_tag>& B,
boost::mpl::false_, row_major
) {
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size2() == B().size1());
typedef typename MatA::value_type value_type;
std::size_t size1 = B().size1();
for (std::size_t n = 0; n < size1; ++ n) {
for (std::size_t m = 0; m < n; ++m) {
noalias(row(B(),n)) -= A()(n,m)*row(B(),m);
}
if(!Unit){
RANGE_CHECK(A()(n, n) != value_type());//matrix is singular
row(B(),n)/=A()(n, n);
}
}
}
void dot(
vector_expression<E1> const& e1,
vector_expression<E2> const& e2,
result_type& result
) {
SIZE_CHECK(e1().size() == e2().size());
bindings::dot(
e1, e2,result,
typename bindings::has_optimized_dot<E1,E2,result_type>::type()
);
}
void trsv_impl(
matrix_expression<MatA, cpu_tag> const& A,
vector_expression<V, cpu_tag> &b,
boost::mpl::false_, row_major
) {
SIZE_CHECK(A().size1() == A().size2());
SIZE_CHECK(A().size2() == b().size());
typedef typename MatA::value_type value_type;
std::size_t size = b().size();
for (std::size_t n = 0; n < size; ++ n) {
auto matRow = row(A(),n);
b()(n) -= inner_prod(subrange(matRow,0,n),subrange(b(),0,n));
if(!Unit){
RANGE_CHECK(A()(n, n) != value_type());//matrix is singular
b()(n) /= A()(n, n);
}
}
}
