text:
/*
This file is a part of NNTL project (https://github.com/Arech/nntl)

Copyright (c) 2015-2021, Arech (aradvert@gmail.com; https://github.com/Arech)
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of NNTL nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once

#include <cblas.h>
//TODO: function definitions (like dgemm()) conflicts with similar function definitions in ACML. It builds successfully, but
//links to wrong library and access violation happens in run-time.

#include <complex>
#define lapack_complex_float ::std::complex<float>
#define lapack_complex_double ::std::complex<double>
#include <lapacke.h>

//#include "../../../utils/denormal_floats.h"

#pragma comment(lib,"libopenblas.dll.a")

//what else to do to use OpenBLAS:
// -in the Solution's VC++ Directories property page set parameter Library Directories to point to a folder with the libopenblas.dll.a file
// -copy correct libopenblas.dll and another dlls that the libopenblas.dll require to the debug/release solution's folder
// -if you're going to use any calling convetion except for __cdecl, then most likely, you'll have to update function declarations
//		within the cblas.h and other blas's .h files included to contain the __cdecl keyword (it's absent for some reason).
//		Check the b_OpenBLAS:: methods to find out which function definitions should be changed.

//http://www.christophlassner.de/using-blas-from-c-with-row-major-data.html

// BTW: lda,ldb,ldc is a "major stride". The stride represents the distance in memory between elements in adjacent rows
// (if row-major) or in adjacent columns (if column-major). This means that the stride is usually equal to the number
// of rows/columns in the matrix.
// Matrix A = [1 2 3]
//            [4 5 6]
// Row-major stores values as {1,2,3,4,5,6} Stride here is 3
// Col-major stores values as {1,4,2,5,3,6} Stride here is 2
// (https://www.physicsforums.com/threads/understanding-blas-dgemm-in-c.543110/)

namespace nntl {
namespace math {

	// wrapper around BLAS API. Should at least isolate from double/float differences
	// Also, we are going to use ColMajor ordering in all math libraries (most of them use it by default)
	// EXPECTING data to be in COL-MAJOR mode!
	//
	// NB: Leading dimension is the number of elements in major dimension. We're using Col-Major ordering,
	// therefore it is the number of ROWs of a matrix
	struct b_OpenBLAS {
	private:
		typedef utils::_scoped_restore_FPU _restoreFPU;
	public:
		//TODO: beware that sz_t type used as substitution of blasint can overflow blasint and silencing conversion warnings here can make it difficult to debug!
		//TODO: May be there should be some preliminary check for this condition.

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		// LEVEL 1
		// AXPY y=a*x+y
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value > axpy(
			const sz_t n, const fl_t alpha, const fl_t *x, const sz_t incx, fl_t *y, const sz_t incy)
		{
			_restoreFPU r;
			cblas_daxpy(static_cast<blasint>(n), alpha, x, static_cast<blasint>(incx), y, static_cast<blasint>(incy));
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value > axpy(
			const sz_t n, const fl_t alpha, const fl_t *x, const sz_t incx, fl_t *y, const sz_t incy)
		{
			_restoreFPU r;
			cblas_saxpy(static_cast<blasint>(n), alpha, x, static_cast<blasint>(incx), y, static_cast<blasint>(incy));
		}

		//////////////////////////////////////////////////////////////////////////
		// cblas_?dot
		// Computes a vector - vector dot product.
		//	Input Parameters
		// n - Specifies the number of elements in vectors x and y.
		// x - Array, size at least(1 + (n - 1)*abs(incx)).
		// incx - Specifies the increment for the elements of x.
		// y - Array, size at least(1 + (n - 1)*abs(incy)).
		// incy - Specifies the increment for the elements of y.
		// Return Values - The result of the dot product of x and y, if n is positive.Otherwise, returns 0.
		//https://software.intel.com/en-us/mkl-developer-reference-c-cblas-dot
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value, double >
			dot(const sz_t n, const fl_t *x, const sz_t incx, const fl_t *y, const sz_t incy)
		{
			_restoreFPU r;
			return cblas_ddot(static_cast<blasint>(n), x, static_cast<blasint>(incx), y, static_cast<blasint>(incy));
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value, float >
			dot(const sz_t n, const fl_t *x, const sz_t incx, const fl_t *y, const sz_t incy)
		{
			_restoreFPU r;
			return cblas_sdot(static_cast<blasint>(n), x, static_cast<blasint>(incx), y, static_cast<blasint>(incy));
		}

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		// LEVEL 2

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		// LEVEL 3
		//
		//////////////////////////////////////////////////////////////////////////
		// General matrix multiplication
		// GEMM C := alpha*op(A)*op(B) + beta*C
		// https://software.intel.com/en-us/node/520775
		// where:
		// op(X) is one of op(X) = X, or op(X) = XT, or op(X) = XH,
		// alpha and beta are scalars,
		// A, B and C are matrices :
		// op(A) is an m - by - k matrix,
		// op(B) is a k - by - n matrix,
		// C is an m - by - n matrix.
		//
		// M - Specifies the number of rows of the matrix op(A) and of the matrix C. The value of m must be at least zero.
		// N - Specifies the number of columns of the matrix op(B) and the number of columns of the matrix C. The value of n must be at least zero.
		// K - Specifies the number of columns of the matrix op(A) and the number of rows of the matrix op(B). The value of k must be at least zero.
		// A - {transa=CblasNoTrans : Array, size lda*k. Before entry, the leading m-by-k part of the array a must contain the matrix A.
		//		transa=CblasTrans : Array, size lda*m. Before entry, the leading k-by-m part of the array a must contain the matrix A.}
		// lda - Specifies the leading dimension of A as declared in the calling (sub)program.
		//		{transa=CblasNoTrans, lda must be at least max(1, m).
		//		transa=CblasTrans, lda must be at least max(1, k)}
		// B - {transb=CblasNoTrans : Array, size ldb by n. Before entry, the leading k-by-n part of the array b must contain the matrix B.
		//		transb=CblasTrans : Array, size ldb by k. Before entry the leading n-by-k part of the array b must contain the matrix B.}
		// ldb - Specifies the leading dimension of B as declared in the calling (sub)program.
		//		{transb = CblasNoTrans : ldb must be at least max(1, k).
		//		transb=CblasTrans : ldb must be at least max(1, n).}
		// C - Array, size ldc by n. Before entry, the leading m-by-n part of the array c must contain the matrix C, except when beta is
		//		equal to zero, in which case c need not be set on entry.
		// ldc - Specifies the leading dimension of c as declared in the calling (sub)program. ldc must be at least max(1, m).
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value >
			gemm( //const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB,
			const bool bTransposeA, const bool bTransposeB,
			const sz_t M, const sz_t N, const sz_t K, const fl_t alpha, const fl_t *A, const sz_t lda,
			const fl_t *B, const sz_t ldb, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_dgemm(CblasColMajor, bTransposeA ? CblasTrans : CblasNoTrans, bTransposeB ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(M), static_cast<blasint>(N), static_cast<blasint>(K),
				alpha, A, static_cast<blasint>(lda), B, static_cast<blasint>(ldb), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value >
			gemm( //const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB,
			const bool bTransposeA, const bool bTransposeB,
			const sz_t M, const sz_t N, const sz_t K, const fl_t alpha, const fl_t *A, const sz_t lda,
			const fl_t *B, const sz_t ldb, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_sgemm(CblasColMajor, bTransposeA ? CblasTrans: CblasNoTrans, bTransposeB ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(M), static_cast<blasint>(N), static_cast<blasint>(K),
				alpha, A, static_cast<blasint>(lda), B, static_cast<blasint>(ldb), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}

		//////////////////////////////////////////////////////////////////////////
		// cblas_?syrk, https://software.intel.com/en-us/node/520780
		// Performs a symmetric rank-k update.
		// The ?syrk routines perform a rank-k matrix-matrix operation for a symmetric matrix C using a general matrix A.
		// The operation is defined as:
		// C := alpha*A*A' + beta*C,
		//		or
		// C : = alpha*A'*A + beta*C,
		// where :
		//		alpha and beta are scalars,
		//		C is an n-by-n symmetric matrix,
		//		A is an n-by-k matrix in the first case and a k-by-n matrix in the second case.
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value >
			syrk( const bool bCLowerTriangl, const bool bFirstATransposed, const sz_t N, const sz_t K, const fl_t alpha
				, const fl_t *A, const sz_t lda, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_dsyrk(CblasColMajor, bCLowerTriangl ? CblasLower : CblasUpper, bFirstATransposed ? CblasTrans : CblasNoTrans
				, static_cast<blasint>(N), static_cast<blasint>(K), alpha, A, static_cast<blasint>(lda), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value >
			syrk(const bool bCLowerTriangl, const bool bFirstATransposed, const sz_t N, const sz_t K, const fl_t alpha
				, const fl_t *A, const sz_t lda, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_ssyrk(CblasColMajor, bCLowerTriangl ? CblasLower : CblasUpper, bFirstATransposed ? CblasTrans : CblasNoTrans
				, static_cast<blasint>(N), static_cast<blasint>(K), alpha, A, static_cast<blasint>(lda), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}

		//////////////////////////////////////////////////////////////////////////
		// cblas_?symm, https://software.intel.com/en-us/node/520779
		// Computes a matrix - matrix product where one input matrix is symmetric.
		// The ?symm routines compute a scalar-matrix-matrix product with one symmetric matrix and add the
		// result to a scalar-matrix product. The operation is defined as
		// C: = alpha*A*B + beta*C,
		//		or
		// C : = alpha*B*A + beta*C,
		// where :
		//		alpha and beta are scalars,
		//		A is a symmetric matrix,
		//		B and C are m - by - n matrices.
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value >
			symm(const bool bSymmAatLeft, const bool bALowerTriangl, const sz_t M, const sz_t N, const fl_t alpha
				, const fl_t *A, const sz_t lda, const fl_t *B, const sz_t ldb, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_dsymm(CblasColMajor, bSymmAatLeft ? CblasLeft : CblasRight, bALowerTriangl ? CblasLower : CblasUpper
				, static_cast<blasint>(M), static_cast<blasint>(N), alpha, A, static_cast<blasint>(lda)
				, B, static_cast<blasint>(ldb), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value >
			symm(const bool bSymmAatLeft, const bool bALowerTriangl, const sz_t M, const sz_t N, const fl_t alpha
				, const fl_t *A, const sz_t lda, const fl_t *B, const sz_t ldb, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_ssymm(CblasColMajor, bSymmAatLeft ? CblasLeft : CblasRight, bALowerTriangl ? CblasLower : CblasUpper
				, static_cast<blasint>(M), static_cast<blasint>(N), alpha, A, static_cast<blasint>(lda)
				, B, static_cast<blasint>(ldb), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		// LAPACKE

		// ?gesvd
		// https://software.intel.com/en-us/node/521150
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value, int>
			gesvd(/*int matrix_layout,*/ const char jobu, const char jobvt,
				const sz_t m, const sz_t n, fl_t* A,
				const sz_t lda, fl_t* S, fl_t* U, const sz_t ldu,
				fl_t* Vt, const sz_t ldvt, fl_t* superb)
		{
			_restoreFPU r;
			return static_cast<int>(LAPACKE_dgesvd(LAPACK_COL_MAJOR, jobu, jobvt, static_cast<lapack_int>(m), static_cast<lapack_int>(n)
				, A, static_cast<lapack_int>(lda), S, U, static_cast<lapack_int>(ldu), Vt, static_cast<lapack_int>(ldvt), superb));
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value, int>
			gesvd(/*int matrix_layout,*/ const char jobu, const char jobvt,
				const sz_t m, const sz_t n, fl_t* A,
				const sz_t lda, fl_t* S, fl_t* U, const sz_t ldu,
				fl_t* Vt, const sz_t ldvt, fl_t* superb)
		{
			_restoreFPU r;
			return static_cast<int>(LAPACKE_sgesvd(LAPACK_COL_MAJOR, jobu, jobvt, static_cast<lapack_int>(m), static_cast<lapack_int>(n)
				, A, static_cast<lapack_int>(lda), S, U, static_cast<lapack_int>(ldu), Vt, static_cast<lapack_int>(ldvt), superb));
		}

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		// Extensions

		// The omatcopy routine performs scaling and out-of-place transposition/copying of matrices. A transposition
		// operation can be a normal matrix copy, a transposition, a conjugate transposition, or just a conjugation.
		// The operation is defined as follows:
		// B : = alpha*op(A)
		//
		// Parameters
		// rows - The number of rows in the source matrix.
		// cols - The number of columns in the source matrix.
		// alpha - This parameter scales the input matrix by alpha.
		// pA - Array.
		// lda - Distance between the first elements in adjacent columns(in the case of the column - major order)
		//		or rows(in the case of the row - major order) in the source matrix; measured in the number of elements.
		//		This parameter must be at least max(1, rows) if ordering = 'C' or 'c', and max(1, cols) otherwise.
		// b - Array.
		// ldb - Distance between the first elements in adjacent columns(in the case of the column - major order)
		//		or rows(in the case of the row - major order) in the destination matrix; measured in the number of elements.
		//		To determine the minimum value of ldb on output, consider the following guideline :
		//		If ordering = 'C' or 'c', then
		//			If trans = 'T' or 't' or 'C' or 'c', this parameter must be at least max(1, cols)
		//			If trans = 'N' or 'n' or 'R' or 'r', this parameter must be at least max(1, rows)
		//		If ordering = 'R' or 'r', then
		//			If trans = 'T' or 't' or 'C' or 'c', this parameter must be at least max(1, rows)
		//			If trans = 'N' or 'n' or 'R' or 'r', this parameter must be at least max(1, cols)
		//
		// #warning current OpenBLAS implementation is slower, than it can be. See TEST(TestPerfDecisions, mTranspose) in test_perf_decisions.cpp
		//and https://github.com/xianyi/OpenBLAS/issues/1243
		// https://github.com/xianyi/OpenBLAS/issues/2532
		// https://stackoverflow.com/questions/16737298/what-is-the-fastest-way-to-transpose-a-matrix-in-c/16743203#16743203
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value >
			omatcopy(const bool bTranspose, const sz_t rows, const sz_t cols, const fl_t alpha,
				const fl_t* pA, const sz_t lda, fl_t* pB, const sz_t ldb)
		{
			_restoreFPU r;
			cblas_domatcopy(CblasColMajor, bTranspose ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(rows), static_cast<blasint>(cols), alpha,
				pA, static_cast<blasint>(lda), pB, static_cast<blasint>(ldb));
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value >
			omatcopy(const bool bTranspose, const sz_t rows, const sz_t cols, const fl_t alpha,
				const fl_t* pA, const sz_t lda, fl_t* pB, const sz_t ldb)
		{
			_restoreFPU r;
			cblas_somatcopy(CblasColMajor, bTranspose ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(rows), static_cast<blasint>(cols), alpha,
				pA, static_cast<blasint>(lda), pB, static_cast<blasint>(ldb));
		}

		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value >
			imatcopy(const bool bTranspose, const sz_t rows, const sz_t cols, const fl_t alpha,
				fl_t* pA, const sz_t lda, const sz_t ldb)
		{
			_restoreFPU r;
			cblas_dimatcopy(CblasColMajor, bTranspose ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(rows), static_cast<blasint>(cols), alpha,
				pA, static_cast<blasint>(lda), static_cast<blasint>(ldb));
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value >
			imatcopy(const bool bTranspose, const sz_t rows, const sz_t cols, const fl_t alpha,
				fl_t* pA, const sz_t lda, const sz_t ldb)
		{
			_restoreFPU r;
			cblas_simatcopy(CblasColMajor, bTranspose ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(rows), static_cast<blasint>(cols), alpha,
				pA, static_cast<blasint>(lda), static_cast<blasint>(ldb));
		}
	};

}
}

max_stars_repo_name: Arech/nntl

max_issues_repo_name: Arech/nntl

max_forks_repo_name: Arech/nntl


text:
#pragma once

#include <gsl-lite/gsl-lite.hpp>

#include <cuda/define_specifiers.hpp>
#include <cuda/runtime_api.hpp>

#include <thrustshift/math.h>

namespace thrustshift {

namespace device_function {

namespace explicit_unroll {

/*! \brief Fill range of length `N` with `x`.
 *
 *  \param N length of the range
 *  \param p pointer to the range
 *  \param x value
 *  \param tid ID of the thread which enters the function
 *  \param num_threads total amount of threads which enter the function
 */
template <typename T, int num_threads, int N>
CUDA_FD void fill(T* p, T x, int tid) {
	constexpr int num_elements_per_thread = N / num_threads;
#pragma unroll
	for (int i = 0; i < num_elements_per_thread; ++i) {
		p[i * num_threads + tid] = x;
	}
	constexpr int num_rest = N % num_threads;
	if (tid < num_rest) {
		p[num_elements_per_thread * num_threads + tid] = x;
	}
}

} // namespace explicit_unroll

namespace implicit_unroll {

template <typename T0, typename T1, typename I0, typename I1, typename I2>
CUDA_FD void fill(T0* p, T1 x, I0 tid, I1 num_threads, I2 N) {
	auto num_elements_per_thread = N / num_threads;
#pragma unroll
	for (int i = 0; i < num_elements_per_thread; ++i) {
		p[i * num_threads + tid] = x;
	}
	auto num_rest = N % num_threads;
	if (tid < num_rest) {
		p[num_elements_per_thread * num_threads + tid] = x;
	}
}

} // namespace implicit_unroll

} // namespace device_function

namespace kernel {
template <typename T, class Range>
__global__ void fill(Range r, T val) {

	const auto gtid = threadIdx.x + blockIdx.x * blockDim.x;
	if (gtid < r.size()) {
		r[gtid] = val;
	}
}

} // namespace kernel

namespace async {

template <typename T, class Range>
void fill(cuda::stream_t& stream, Range&& r, T val) {

	constexpr cuda::grid::block_dimension_t block_dim = 256;
	const cuda::grid::dimension_t grid_dim =
	    ceil_divide(r.size(), gsl_lite::narrow<decltype(r.size())>(block_dim));

	using RangeT = typename std::remove_reference<Range>::type::value_type;

	if (!r.empty()) {
		cuda::enqueue_launch(kernel::fill<T, gsl_lite::span<RangeT>>,
		                     stream,
		                     cuda::make_launch_config(grid_dim, block_dim),
		                     r,
		                     val);
	}
}

} // namespace async

} // namespace thrustshift

ext: h

max_stars_repo_name: pauleonix/thrustshift

max_issues_repo_name: pauleonix/thrustshift

max_forks_repo_name: pauleonix/thrustshift


text:
/**
 * Simple and fast kmeans-implementation making use of multithreading.
 *
 * Author: Anders Bennehag
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 Anders Bennehag
 *
 */
#ifndef FASTKMEANS_H_
#define FASTKMEANS_H_

#include <gsl/gsl_matrix.h>

int fkm_kmeans(const gsl_matrix* points, gsl_matrix* clusters,
               size_t max_iter, int num_threads);

gsl_matrix* fkm_matrix_load(FILE* fout);
int fkm_matrix_save(FILE* fout, gsl_matrix* mat);

#define WHERESTR  "[%s:%d]: "
#define WHEREARG  __FILE__, __LINE__
#define DEBUGPRINT2(...)       fprintf(stderr, __VA_ARGS__)

#define FKM_LOGFMT(_fmt, ...)  DEBUGPRINT2(WHERESTR _fmt "\n", WHEREARG, __VA_ARGS__)
#define FKM_LOG(_s) DEBUGPRINT2(WHERESTR "%s\n", WHEREARG, _s)
#define FKM_LOGTYPE(_tp, _s)  DEBUGPRINT2(WHERESTR "%s: %s" "\n", WHEREARG, _tp, _s)
#define FKM_LOGTYPEFMT(_tp, _fmt, ...)  DEBUGPRINT2(WHERESTR "%s: " _fmt "\n", WHEREARG, _tp, __VA_ARGS__)

#if (ISDEBUG + 0)
    #define FKM_DEBUGFMT(_fmt, ...)  FKM_LOGTYPEFMT("DEBUG", _fmt, __VA_ARGS__)
    #define FKM_DEBUG(_s)  FKM_LOGTYPE("DEBUG", _s)
#else
    #define FKM_DEBUGFMT(...)
    #define FKM_DEBUG(...)
#endif
#define FKM_INFOFMT(_fmt, ...)   FKM_LOGTYPEFMT("INFO", _fmt, __VA_ARGS__)
#define FKM_WARNFMT(_fmt, ...)   FKM_LOGTYPEFMT("WARN", _fmt, __VA_ARGS__)
#define FKM_ERRORFMT(_fmt, ...)  FKM_LOGTYPEFMT("ERROR", _fmt, __VA_ARGS__)

#define FKM_INFO(_s)   FKM_LOGTYPE("INFO", _s)
#define FKM_WARN(_s)   FKM_LOGTYPE("WARN", _s)
#define FKM_ERROR(_s)  FKM_LOGTYPE("ERROR", _s)

#endif

max_stars_repo_name: PureW/fast-kmeans

max_issues_repo_name: PureW/fast-kmeans

max_forks_repo_name: PureW/fast-kmeans


text:
#include <gsl/gsl_math.h>
#include "gsl_cblas.h"
#include "cblas.h"

double
cblas_ddot (const int N, const double *X, const int incX, const double *Y,
	    const int incY)
{
#define INIT_VAL  0.0
#define ACC_TYPE  double
#define BASE double
#include "source_dot_r.h"
#undef ACC_TYPE
#undef BASE
#undef INIT_VAL
}

ext: c


text:
/*
 * Copyright 2020-2021 OpenDR European Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef SCANMATCHERPROCESSOR_H
#define SCANMATCHERPROCESSOR_H

#include <gmapping/log/sensorlog.h>
#include <gmapping/sensor/sensor_range/rangereading.h>
#include <gmapping/sensor/sensor_range/rangesensor.h>
//#include <gsl/gsl_eigen.h>
#include "gmapping/scanmatcher/scanmatcher.h"

namespace GMapping {

  class ScanMatcherProcessor {
  public:
    ScanMatcherProcessor(const ScanMatcherMap &m);

    ScanMatcherProcessor(double xmin, double ymin, double xmax, double ymax, double delta, double patchdelta);

    virtual ~ScanMatcherProcessor();

    virtual void processScan(const RangeReading &reading);

    void setSensorMap(const SensorMap &smap, std::string sensorName = "FLASER");

    void init();

    void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt,
                               int iterations, bool computeCovariance = false);

    void setRegistrationParameters(double regScore, double critScore);

    OrientedPoint getPose() const;

    inline const ScanMatcherMap &getMap() const { return m_map; }

    inline ScanMatcher &matcher() { return m_matcher; }

    inline void setmaxMove(double mmove) { m_maxMove = mmove; }

    bool useICP;

  protected:
    ScanMatcher m_matcher;
    bool m_computeCovariance;
    bool m_first;
    SensorMap m_sensorMap;
    double m_regScore, m_critScore;
    unsigned int m_beams;
    double m_maxMove;
    // state
    ScanMatcherMap m_map;
    OrientedPoint m_pose;
    OrientedPoint m_odoPose;
    int m_count;
    // gsl_eigen_symmv_workspace * m_eigenspace;
  };

};  // namespace GMapping

#endif

max_stars_repo_name: makistsantekidis/opendr

max_issues_repo_name: makistsantekidis/opendr

max_forks_repo_name: makistsantekidis/opendr


text:
/*
** Implementation of LISA algorithm
** for statistical inference of fMRI images
**
** 2nd level inference (one-sample test)
**
** G.Lohmann, 2017
*/
#include <viaio/Vlib.h>
#include <viaio/file.h>
#include <viaio/mu.h>
#include <viaio/option.h>
#include <viaio/os.h>
#include <viaio/VImage.h>

#include <gsl/gsl_cdf.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_histogram.h>

#include <math.h>
#include <stdio.h>
#include <string.h>

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

#define ABS(x) ((x) > 0 ? (x) : -(x))

extern void   VIsolatedVoxels(VImage src,float threshold);
extern void   VHistogram(gsl_histogram *histogram,VString filename);
extern void   VCheckImage(VImage src);
extern void   FDR(VImage src,VImage dest,gsl_histogram *nullhist,gsl_histogram *realhist,double);
extern double ttest1(double *data1,int n);
extern void   ImageStats(VImage src,double *,double *,double *hmin,double *hmax);
extern void   VBilateralFilter(VImage src,VImage dest,int radius,double var1,double var2,int);
extern double VImageVar(VImage src);
extern void   VImageCount(VImage src);
extern void   VGetHistRange(VImage src,double *hmin,double *hmax);
extern void   VZScale(VImage src,float mode,float stddev);
extern float  VGetMode(VImage src);
extern double t2z(double,double);
extern void   HistoUpdate(VImage,gsl_histogram *);

/* generate permutation table */
int **genperm(gsl_rng *rx,int n,int numperm)
{
  int i,j;
  int **table = (int **) VCalloc(numperm,sizeof(int *));
  for (i = 0; i < numperm; i++) {
    table[i] = (int *) VCalloc(n,sizeof(int));
    for (j=0; j<n; j++) {
      table[i][j] = 0;
      if (gsl_ran_bernoulli (rx,(double)0.5) == 1) table[i][j] = 1;
    }
  }
  return table;
}

/* onesample t-test */
void OnesampleTest(VImage *src1,VImage dest,int *permtable,int n)
{
  int i,k,b,r,c,nslices,nrows,ncols;
  double nx,ave,var,u,t,z,df;
  double tiny=1.0e-8;
  extern void avevar(double *data,int n,double *a,double *v);

  nslices = VImageNBands(src1[0]);
  nrows   = VImageNRows(src1[0]);
  ncols   = VImageNColumns(src1[0]);
  VFillImage(dest,VAllBands,0);
  double *data = (double *) VCalloc(n,sizeof(double));

  for (b=0; b<nslices; b++) {
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {
	k = 0;
	for (i=0; i<n; i++) {
	  u = (double)VPixel(src1[i],b,r,c,VFloat);
	  if (ABS(u) > tiny) {
	    if (permtable[i] > 0) u = -u;
	    data[k] = u;
	    k++;
	  }
	}
	if (k < n-2) continue;
	avevar(data,k,&ave,&var);
	if (var < tiny) continue;
	nx   = (double)k;
	t    = sqrt(nx) * ave/sqrt(var);
	df   = nx - 1.0;
	z    = t2z(t,df);
	if (t < 0) z = -z;
	VPixel(dest,b,r,c,VFloat) = z;
      }
    }
  }
  VFree(data);
}

int main (int argc, char *argv[])
{
  static VArgVector in_files1;
  static VString  out_filename="";
  static VString  mask_filename="";
  static VFloat   alpha = 0.05;
  static VShort   radius = 2;
  static VFloat   rvar = 2.0;
  static VFloat   svar = 2.0;
  static VShort   numiter = 2;
  static VShort   numperm = 5000;
  static VLong    seed = 99402622;
  static VBoolean centering = FALSE;
  static VBoolean cleanup = TRUE;
  static VShort   nproc = 0;
  static VOptionDescRec options[] = {
    {"in", VStringRepn, 0, & in_files1, VRequiredOpt, NULL,"Input files" },
    {"out", VStringRepn, 1, & out_filename, VRequiredOpt, NULL,"Output file" },
    {"alpha",VFloatRepn,1,(VPointer) &alpha,VOptionalOpt,NULL,"FDR significance level"},
    {"perm",VShortRepn,1,(VPointer) &numperm,VOptionalOpt,NULL,"Number of permutations"},
    {"mask", VStringRepn, 1, (VPointer) &mask_filename, VRequiredOpt, NULL, "Mask"},
    {"seed",VLongRepn,1,(VPointer) &seed,VOptionalOpt,NULL,"Seed for random number generation"},
    {"radius",VShortRepn,1,(VPointer) &radius,VOptionalOpt,NULL,"Bilateral parameter (radius in voxels)"},
    {"rvar",VFloatRepn,1,(VPointer) &rvar,VOptionalOpt,NULL,"Bilateral parameter (radiometric)"},
    {"svar",VFloatRepn,1,(VPointer) &svar,VOptionalOpt,NULL,"Bilateral parameter (spatial)"},
    {"filteriterations",VShortRepn,1,(VPointer) &numiter,VOptionalOpt,NULL,"Bilateral parameter (number of iterations)"},
    {"cleanup",VBooleanRepn,1,(VPointer) &cleanup,VOptionalOpt,NULL,"Whether to remove isloated voxels"},
    {"j",VShortRepn,1,(VPointer) &nproc,VOptionalOpt,NULL,"Number of processors to use, '0' to use all"},
  };

  FILE *fp=NULL;
  VString in_filename;
  VAttrList list1=NULL,out_list=NULL,geolist=NULL;
  VImage *src1;
  int i,nimages,npix=0;
  char *prg_name=GetLipsiaName("vlisa_onesample");
  fprintf (stderr, "%s\n", prg_name);

  /* parse command line */
  if (! VParseCommand (VNumber (options), options, & argc, argv)) {
    VReportUsage (argv[0], VNumber (options), options, NULL);
    exit (EXIT_FAILURE);
  }
  if (argc > 1) {
    VReportBadArgs (argc, argv);
    exit (EXIT_FAILURE);
  }

  /* omp-stuff */
#ifdef _OPENMP
  int num_procs=omp_get_num_procs();
  if (nproc > 0 && nproc < num_procs) num_procs = nproc;
  fprintf(stderr," using %d cores\n",(int)num_procs);
  omp_set_num_threads(num_procs);
#endif /* _OPENMP */

  /* images  */
  VImage mask = VReadImageFile(mask_filename);
  if (mask==NULL) VError("Error reading mask file %s",mask_filename);

  nimages = in_files1.number;
  src1 = (VImage *) VCalloc(nimages,sizeof(VImage));
  for (i = 0; i < nimages; i++) {
    in_filename = ((VString *) in_files1.vector)[i];
    list1   = VReadAttrList(in_filename,0L,TRUE,FALSE);
    VMaskMinval(list1,mask,0.0);

    src1[i] = VReadImage(list1);
    if (src1[i] == NULL) VError(" no input image found");
    if (VPixelRepn(src1[i]) != VFloatRepn) VError(" input pixel repn must be float");
    if (i == 0) npix = VImageNPixels(src1[i]);
    else if (npix != VImageNPixels(src1[i])) VError(" inconsistent image dimensions");

    /* use geometry info from 1st file */
    if (geolist == NULL) {
      geolist = VGetGeoInfo(list1);
      double *D = VGetGeoPixdim(geolist,NULL);
      if (fabs(D[0]-4.0) < TINY) VError(" The input must be a list of separate 3D images, not one 4D file");
    }
  }

  /* ini random permutations */
  size_t n = nimages;
  gsl_rng_env_setup();
  const gsl_rng_type *T = gsl_rng_default;
  gsl_rng *rx = gsl_rng_alloc(T);
  gsl_rng_set(rx,(unsigned long int)seed);
  int nperm=0;
  int **permtable = genperm(rx,(int)n,(int)numperm);

  /* estimate null variance based on first 30 permutations */
  double hmin=0,hmax=0;
  float stddev=1.0;
  if (numperm > 0) {
    int tstperm = 30;
    if (tstperm > numperm) tstperm = numperm;
    double varsum=0,nx=0;

#pragma omp parallel for shared(src1,permtable) schedule(dynamic)
    for (nperm = 0; nperm < tstperm; nperm++) {
      VImage zmap = VCreateImageLike(src1[0]);
      OnesampleTest(src1,zmap,permtable[nperm],nimages);
#pragma omp critical
      {
	varsum += VImageVar(zmap);
	nx++;
      }
      VDestroyImage(zmap);
    }
    double meanvar = varsum/nx;
    stddev = sqrt(meanvar);
  }

  /* no permutation */
  int *nopermtable = (int *) VCalloc(n,sizeof(int));
  VImage dst1  = VCreateImageLike (src1[0]);
  VImage zmap1 = VCreateImageLike(src1[0]);
  OnesampleTest(src1,zmap1,nopermtable,nimages);

  if (numperm == 0) {
    double z = VImageVar(zmap1);
    stddev = (float)(sqrt(z)); /* update stddev */
  }
  float mode=0;
  if (centering) mode = VGetMode(zmap1);
  if (numperm > 0) VZScale(zmap1,mode,stddev);
  VBilateralFilter(zmap1,dst1,(int)radius,(double)(rvar),(double)svar,(int)numiter);

  /* ini histograms */
  VGetHistRange(dst1,&hmin,&hmax);
  size_t nbins = 20000;
  gsl_histogram *hist0 = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (hist0,hmin,hmax);
  gsl_histogram *histz = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (histz,hmin,hmax);
  HistoUpdate(dst1,histz);

  /* random permutations */
#pragma omp parallel for shared(src1,permtable) schedule(dynamic)
  for (nperm = 0; nperm < numperm; nperm++) {
    if (nperm%20 == 0) fprintf(stderr," perm  %4d  of  %d\r",nperm,(int)numperm);

    VImage zmap = VCreateImageLike(src1[0]);
    VImage dst  = VCreateImageLike (zmap);
    OnesampleTest(src1,zmap,permtable[nperm],nimages);
    float mode=0;
    if (centering) mode = VGetMode(zmap);
    VZScale(zmap,mode,stddev);
    VBilateralFilter(zmap,dst,(int)radius,(double)(rvar),(double)svar,(int)numiter);

#pragma omp critical
    {
      HistoUpdate(dst,hist0);
    }
    VDestroyImage(dst);
    VDestroyImage(zmap);
  }

  /* apply fdr */
  VImage fdrimage = VCopyImage (dst1,NULL,VAllBands);
  if (numperm > 0) {
    FDR(dst1,fdrimage,hist0,histz,(double)alpha);

    if (cleanup && alpha < 1.0) {
      VIsolatedVoxels(fdrimage,(float)(1.0-alpha));
    }
    if (alpha < 1.0) VImageCount(fdrimage);
  }

  /* write output to disk */
  out_list = VCreateAttrList ();
  VHistory(VNumber(options),options,prg_name,&list1,&out_list);
  VSetGeoInfo(geolist,out_list);
  VAppendAttr (out_list,"image",NULL,VImageRepn,fdrimage);
  fp = VOpenOutputFile (out_filename, TRUE);
  if (! VWriteFile (fp, out_list)) exit (1);
  fclose(fp);
  fprintf (stderr, "\n%s: done.\n", argv[0]);
  exit(0);
}

max_stars_repo_name: zrajna/lipsia

max_issues_repo_name: zrajna/lipsia

max_forks_repo_name: zrajna/lipsia


text:
#ifndef TRACKER_GMD_H
#define TRACKER_GMD_H

#include "tracker.h"

#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h> /* GAUSSIAN*/
#include "helper/Constants.h"
#include <limits.h>
#include "helper/high_res_timer.h"
#include "helper/bounding_box_regressor.h"

class TrackerGMD : public Tracker {

public:
  TrackerGMD(const bool show_tracking, ExampleGenerator* example_generator,  RegressorTrainBase* regressor_train);

  // Estimate the location of the target object in the current image.
  virtual void Track(const cv::Mat& image_curr, RegressorBase* regressor,
             BoundingBox* bbox_estimate_uncentered);

  // After tracking for this frame, update internal state
  virtual void UpdateState(const cv::Mat& image_curr, BoundingBox &bbox_estimate, RegressorBase* regressor, bool is_last_frame);

  // Initialize the tracker with the ground-truth bounding box of the first frame.
  virtual void Init(const cv::Mat& image_curr, const BoundingBox& bbox_gt,
            RegressorBase* regressor);

  virtual void Init(const std::string& image_curr_path, const VOTRegion& region,
            RegressorBase* regressor);

  // Online fine tune, given the networks and example_generators
  virtual void FineTuneOnline(ExampleGenerator* example_generator,
                                RegressorTrainBase* regressor_train, bool success_frame, bool is_last_frame);

  // Actual worker to do the finetune
  void FineTuneWorker(ExampleGenerator* example_generator,
                                RegressorTrainBase* regressor_train,
                                std::vector<int> &this_bag,
                                const int pos_candidate_upper_bound = INT_MAX,
                                const int neg_candidate_upper_bound = INT_MAX);

  // Motion Model around bbox_curr_prior_tight_
  void GetCandidates(BoundingBox &cur_bbox, int W, int H, std::vector<BoundingBox> &candidate_bboxes);

  // Check if generated candidate is valid or not
  bool ValidCandidate(BoundingBox &candidate_bbox, int W, int H);

  // Get one moved box, according to the Gaussian Motion Model
  BoundingBox GenerateOneGaussianCandidate(int W, int H, BoundingBox &bbox, double sd_x = SD_X, double sd_y = SD_Y,
                                           double sd_scale = SD_SCALE, double sd_ap = SD_AP);

  // Create and Enqueue Training Samples given already set up example_generator
  virtual void EnqueueOnlineTraningSamples(ExampleGenerator* example_generator, const cv::Mat &image_curr, const BoundingBox &estimate,  bool success_frame);

  // check if the current estimate is success, needed as flag to pass to EnqueueOnlineTraningSamples
  virtual bool IsSuccessEstimate();

  // clear all the related storage for tracking net video
  virtual void Reset(RegressorBase *regressor);

private:
  gsl_rng *rng_;

  // Used to generate additional training examples through synthetic transformations.
  ExampleGenerator* example_generator_;

  // Neural network.
  RegressorTrainBase* regressor_train_;

  // this prediction scores for candidates
  std::vector<float> candidate_probabilities_;
  std::vector<BoundingBox> candidates_bboxes_;
  std::vector<int> sorted_idxes_; // the sorted indexes of probabilities from high to low

  // samples collected along each frame
  std::vector<BoundingBox> gts_;
  std::vector<cv::Mat> image_currs_;
  std::vector<cv::Mat> images_finetune_;
  std::vector<cv::Mat> targets_finetune_;
  std::vector<std::vector<BoundingBox> > candidates_finetune_pos_;
  std::vector<std::vector<BoundingBox> > candidates_finetune_neg_;
  // std::vector<std::vector<double> > labels_finetune_pos_;
  // std::vector<std::vector<double> > labels_finetune_neg_;

  // long term and short term
  std::vector<int> short_term_bag_;
  std::vector<int> long_term_bag_;

  std::mt19937 engine_;

  // for motion model candidates
  double sd_trans_;
  double sd_scale_;
  double sd_ap_;

  // timer
  HighResTimer hrt_;

  // Bbox regressor
  BoundingBoxRegressor bbox_finetuner_;

};

#endif


text:
#include <gsl/gsl_rng.h>
#include <stdio.h>

/*
gsl_rng_mt19937
gsl_rng_ranlxs0
gsl_rng_ranlxs1
gsl_rng_ranlxs2
*/

void print_algos(void) {

    const gsl_rng_type **t, **t0;

    t0 = gsl_rng_types_setup();

    printf("Available generators:\n");

    for (t = t0; *t != 0; t++) {
        printf("%s\n", (*t)->name);
    }

}

int main(void) {
    int seed = 102;
    int n = 10;

    gsl_rng_env_setup();

    gsl_rng* r = gsl_rng_alloc(gsl_rng_mt19937);
    gsl_rng_set(r, seed);

    for (int i = 0; i < n; i++) {
        double u = gsl_rng_uniform(r);
        printf("%.5f\n", u);
    }

    gsl_rng_free(r);

    return 0;
}

max_stars_repo_name: shakfu/pd-psl

max_issues_repo_name: shakfu/pd-psl

max_forks_repo_name: shakfu/pd-psl


text:
#include <stdio.h>
#include <stdlib.h>

#include <gsl/gsl_vector.h>
#include <gsl/gsl_multimin.h>

typedef struct {
    gsl_vector *alpha, *center;
    double offset;
} Params;

/*
 * Function implements a 2-D parabola centered on center, with minimum
 * at offset.
 * */
double parabola(const gsl_vector *v, void *params) {
    double x = gsl_vector_get(v, 0);
    double y = gsl_vector_get(v, 1);
    Params *p = (Params *) params;
    double a = gsl_vector_get(p->alpha, 0);
    double b = gsl_vector_get(p->alpha, 1);
    double c = p->offset;
    x -= gsl_vector_get(p->center, 0);
    y -= gsl_vector_get(p->center, 1);
    return a*x*x + b*y*y + c;
}

int main(int argc, char *argv[]) {
    const int nr_dims = 2, nr_iters = 100;
    /* Use Nelder & Mead simplex method */
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
    gsl_multimin_fminimizer *multimin;
    gsl_multimin_function f;
    gsl_vector *x, *step_size;
    Params params;
    int iter_nr = 0, status;
    double size;

    /* Initialize the parameters */
    params.alpha = gsl_vector_alloc(nr_dims);
    gsl_vector_set(params.alpha, 0, 1.0);
    gsl_vector_set(params.alpha, 1, 2.0);
    params.center = gsl_vector_alloc(nr_dims);
    gsl_vector_set(params.center, 0, 1.5);
    gsl_vector_set(params.center, 1, -0.5);
    params.offset = 3.4;
    f.n = nr_dims;
    f.f = parabola;
    f.params = &params;

    /* Initiallize the initial position */
    x = gsl_vector_alloc(nr_dims);
    gsl_vector_set(x, 0, 2.3);
    gsl_vector_set(x, 1, -1.5);

    /* Initialize the simplex step sizes */
    step_size = gsl_vector_alloc(nr_dims);
    gsl_vector_set_all(step_size, 1.0);

    /* Initialize the minimizer */
    multimin = gsl_multimin_fminimizer_alloc(T, 2);
    gsl_multimin_fminimizer_set(multimin, &f, x, step_size);

    /* Do the iterations until success, or the maximum number of iterations
     * is exceeded. */
    do {
        iter_nr++;
        /* Do an iteration step */
        status = gsl_multimin_fminimizer_iterate(multimin);
        if (status)
            break;
        /* Determine the step size to use a a stopping criterion */
        size = gsl_multimin_fminimizer_size(multimin);
        status = gsl_multimin_test_size(size, 1.0e-4);
        if (status == GSL_SUCCESS) {
            printf("Minimum:\n");
            printf("%4d: f(%.5f, %.5f) = %.5f, last step size = %.5f\n",
                   iter_nr,
                   gsl_vector_get(multimin->x, 0),
                   gsl_vector_get(multimin->x, 1),
                   multimin->fval,
                   size);
            printf("Expected:\n");
            printf("      f(%.5f, %.5f) = %.5f\n",
                   gsl_vector_get(params.center, 0),
                   gsl_vector_get(params.center, 1),
                   params.offset);
        } else {
            fprintf(stderr, "%4d: f(%.5f, %.5f) = %.5f, step size = %.5f\n",
                    iter_nr,
                    gsl_vector_get(multimin->x, 0),
                    gsl_vector_get(multimin->x, 1),
                    multimin->fval,
                    size);
        }
    } while (status == GSL_CONTINUE && iter_nr < nr_iters);

    /* Free everything that was allocated */
    gsl_multimin_fminimizer_free(multimin);
    gsl_vector_free(x);
    gsl_vector_free(step_size);
    gsl_vector_free(params.alpha);
    gsl_vector_free(params.center);
    return EXIT_SUCCESS;
}

max_stars_repo_name: Gjacquenot/training-material

max_issues_repo_name: Gjacquenot/training-material

max_forks_repo_name: Gjacquenot/training-material


text:
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <unistd.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>

#include "core_allvars.h"
#include "core_proto.h"

void init(void)
{
  int i;

  Age = mymalloc(ABSOLUTEMAXSNAPS*sizeof(*Age));

  random_generator = gsl_rng_alloc(gsl_rng_ranlxd1);
  gsl_rng_set(random_generator, 42);	 // start-up seed

  set_units();
  srand((unsigned) time(NULL));

  read_snap_list();

  //Hack to fix deltaT for snapshot 0
  //This way, galsnapnum = -1 will not segfault.
  Age[0] = time_to_present(1000.0);//lookback time from z=1000
  Age++;

  for(i = 0; i < Snaplistlen; i++)
  {
    ZZ[i] = 1 / AA[i] - 1;
    Age[i] = time_to_present(ZZ[i]);
  }

  a0 = 1.0 / (1.0 + Reionization_z0);
  ar = 1.0 / (1.0 + Reionization_zr);

  read_cooling_functions();

}

void set_units(void)
{

  UnitTime_in_s = UnitLength_in_cm / UnitVelocity_in_cm_per_s;
  UnitTime_in_Megayears = UnitTime_in_s / SEC_PER_MEGAYEAR;
  G = GRAVITY / pow(UnitLength_in_cm, 3) * UnitMass_in_g * pow(UnitTime_in_s, 2);
  UnitDensity_in_cgs = UnitMass_in_g / pow(UnitLength_in_cm, 3);
  UnitPressure_in_cgs = UnitMass_in_g / UnitLength_in_cm / pow(UnitTime_in_s, 2);
  UnitCoolingRate_in_cgs = UnitPressure_in_cgs / UnitTime_in_s;
  UnitEnergy_in_cgs = UnitMass_in_g * pow(UnitLength_in_cm, 2) / pow(UnitTime_in_s, 2);

  EnergySNcode = EnergySN / UnitEnergy_in_cgs * Hubble_h;
  EtaSNcode = EtaSN * (UnitMass_in_g / SOLAR_MASS) / Hubble_h;

  // convert some physical input parameters to internal units
  Hubble = HUBBLE * UnitTime_in_s;

  // compute a few quantitites
  RhoCrit = 3 * Hubble * Hubble / (8 * M_PI * G);

}

void read_snap_list(void)
{
  FILE *fd;
  char fname[1000];

  sprintf(fname, "%s", FileWithSnapList);

  if(!(fd = fopen(fname, "r")))
  {
    printf("can't read output list in file '%s'\n", fname);
    ABORT(0);
  }

  Snaplistlen = 0;
  do
  {
    if(fscanf(fd, " %lg ", &AA[Snaplistlen]) == 1)
      Snaplistlen++;
    else
      break;
  }
  while(Snaplistlen < MAXSNAPS);

  fclose(fd);

#ifdef MPI
  if(ThisTask == 0)
#endif
    printf("found %d defined times in snaplist\n", Snaplistlen);
}

double time_to_present(double z)
{
#define WORKSIZE 1000
  gsl_function F;
  gsl_integration_workspace *workspace;
  double time, result, abserr;

  workspace = gsl_integration_workspace_alloc(WORKSIZE);
  F.function = &integrand_time_to_present;

  gsl_integration_qag(&F, 1.0 / (z + 1), 1.0, 1.0 / Hubble,
    1.0e-8, WORKSIZE, GSL_INTEG_GAUSS21, workspace, &result, &abserr);

  time = 1 / Hubble * result;

  gsl_integration_workspace_free(workspace);

  // return time to present as a function of redshift
  return time;
}

double integrand_time_to_present(double a, void *param)
{
  return 1 / sqrt(Omega / a + (1 - Omega - OmegaLambda) + OmegaLambda * a * a);
}

max_stars_repo_name: jacobseiler/SAGE

max_issues_repo_name: jacobseiler/SAGE

max_forks_repo_name: jacobseiler/SAGE


text:
/*
Ballistic: a software to benchmark ballistic models.

AUTHORS: Javier Burguete Tolosa.

Copyright 2018, AUTHORS.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

  1. Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.

  2. Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY AUTHORS ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL AUTHORS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE.
*/

/**
 * \file multi-steps.c
 * \brief Source file to define the multi-steps method data and functions.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2018.
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <libxml/parser.h>
#include "config.h"
#include "utils.h"
#include "equation.h"
#include "method.h"
#include "runge-kutta.h"
#include "multi-steps.h"

#define DEBUG_MULTI_STEPS 0     ///< macro to debug the multi-steps functions.

///> array of a coefficients of the 2nd order multi-steps method.
const long double ms_a2[3] = { 0.75L, 0.L, 0.25L };

///> array of c coefficients of the 2nd order multi-steps method.
const long double ms_c2[3] = { 2.L, 0.L, 0.L };

///> array of error a coefficients of the 2nd order multi-steps method.
const long double ms_ea2[3] = { 0.25L, 0.L, -0.25L };

///> array of error b coefficients of the 2nd order multi-steps method.
const long double ms_eb2[3] = { 0.5L, 0.L, 0.L };

///> array of a coefficients of the 3rd order multi-steps method.
const long double ms_a3[4] = { 16.L / 27.L, 0.L, 0.L, 11.L / 27.L };

///> array of c coefficients of the 3rd order multi-steps method.
const long double ms_c3[4] = { 3.L, 0.L, 0.L, 12.L / 11.L };

///> array of error a coefficients of the 3rd order multi-steps method.
const long double ms_ea3[4] = { 17.L / 108.L, 0.L, 0.25L, -11.L / 27.L };

///> array of error b coefficients of the 3rd order multi-steps method.
const long double ms_eb3[4] = { -5.L / 18.L, 0.L, 0.L, -4.L / 9.L };

/**
 * Function to init the multi-steps method coefficients.
 *
 * \return 1 on success, 0 on error.
 */
int
multi_steps_init (MultiSteps * ms)      ///< MultiSteps struct.
{
  Method *m;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init: start\n");
#endif
  m = MULTI_STEPS_METHOD (ms);
  switch (ms->type)
    {
    case 1:
      method_init (m, 3, 2);
      ms->a = ms_a2;
      ms->c = ms_c2;
      ms->ea = ms_ea2;
      ms->eb = ms_eb2;
      break;
    case 2:
      method_init (m, 4, 3);
      ms->a = ms_a3;
      ms->c = ms_c3;
      ms->ea = ms_ea3;
      ms->eb = ms_eb3;
      break;
    default:
      printf ("Error reading multi-steps data\n");
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_init: error\n");
      fprintf (stderr, "multi_steps_init: end\n");
#endif
      return 0;
    }
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init: success\n");
  fprintf (stderr, "multi_steps_init: end\n");
#endif
  return 1;
}

/**
 * Function to init the variables used by the multi-steps methods.
 */
void
multi_steps_init_variables (MultiSteps * ms)
{
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init_variables: start\n");
#endif
  runge_kutta_init_variables (MULTI_STEPS_RUNGE_KUTTA (ms));
  method_init_variables (MULTI_STEPS_METHOD (ms));
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init_variables: end\n");
#endif
}

/**
 * Function to perform a step of the multi-steps method.
 */
static inline void
multi_steps_step (MultiSteps * ms,      ///< MultiSteps struct.
                  Equation * eq,        ///< Equation struct.
                  long double t,        ///< actual time.
                  long double dt)       ///< time step size.
{
  long double msr0[3], msr1[3];
  Method *m;
  const long double *a;
  const long double *c;
  unsigned int i, n;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_step: start\n");
#endif
  m = MULTI_STEPS_METHOD (ms);
#if DEBUG_MULTI_STEPS
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r0[0][%u]=%Lg\n", i, m->r0[0][i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r1[0][%u]=%Lg\n", i, m->r1[0][i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r2[0][%u]=%Lg\n", i, m->r2[0][i]);
#endif
  n = m->nsteps;
  a = ms->a;
  c = ms->c;
#if DEBUG_MULTI_STEPS
  for (i = 0; i < n; ++i)
    fprintf (stderr, "multi_steps_step: a%u=%Lg\n", i, a[i]);
  for (i = 0; i < n; ++i)
    fprintf (stderr, "multi_steps_step: c%u=%Lg\n", i, c[i]);
#endif
  msr0[0] = a[0] * (r0[0] + dt * c[0] * r1[0]);
  msr0[1] = a[0] * (r0[1] + dt * c[0] * r1[1]);
  msr0[2] = a[0] * (r0[2] + dt * c[0] * r1[2]);
  msr1[0] = a[0] * (r1[0] + dt * c[0] * r2[0]);
  msr1[1] = a[0] * (r1[1] + dt * c[0] * r2[1]);
  msr1[2] = a[0] * (r1[2] + dt * c[0] * r2[2]);
  for (i = 1; i < n; ++i)
    {
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_step: r0[%u][0]=%Lg\n", i, m->r0[i][0]);
      fprintf (stderr, "multi_steps_step: r0[%u][1]=%Lg\n", i, m->r0[i][1]);
      fprintf (stderr, "multi_steps_step: r0[%u][2]=%Lg\n", i, m->r0[i][2]);
      fprintf (stderr, "multi_steps_step: r1[%u][0]=%Lg\n", i, m->r1[i][0]);
      fprintf (stderr, "multi_steps_step: r1[%u][1]=%Lg\n", i, m->r1[i][1]);
      fprintf (stderr, "multi_steps_step: r1[%u][2]=%Lg\n", i, m->r1[i][2]);
      fprintf (stderr, "multi_steps_step: r2[%u][0]=%Lg\n", i, m->r2[i][0]);
      fprintf (stderr, "multi_steps_step: r2[%u][1]=%Lg\n", i, m->r2[i][1]);
      fprintf (stderr, "multi_steps_step: r2[%u][2]=%Lg\n", i, m->r2[i][2]);
#endif
      msr0[0] += a[i] * (m->r0[i][0] + dt * c[i] * m->r1[i][0]);
      msr0[1] += a[i] * (m->r0[i][1] + dt * c[i] * m->r1[i][1]);
      msr0[2] += a[i] * (m->r0[i][2] + dt * c[i] * m->r1[i][2]);
      msr1[0] += a[i] * (m->r1[i][0] + dt * c[i] * m->r2[i][0]);
      msr1[1] += a[i] * (m->r1[i][1] + dt * c[i] * m->r2[i][1]);
      msr1[2] += a[i] * (m->r1[i][2] + dt * c[i] * m->r2[i][2]);
    }
#if DEBUG_MULTI_STEPS
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r0[0][%u]=%Lg\n", i, msr0[i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r1[0][%u]=%Lg\n", i, msr1[i]);
#endif
  memcpy (m->r0[0], r0, 3 * sizeof (long double));
  memcpy (m->r1[0], r1, 3 * sizeof (long double));
  memcpy (m->r2[0], r2, 3 * sizeof (long double));
  for (i = n; --i > 0;)
    {
      memcpy (m->r0[i], m->r0[i - 1], 3 * sizeof (long double));
      memcpy (m->r1[i], m->r1[i - 1], 3 * sizeof (long double));
      memcpy (m->r2[i], m->r2[i - 1], 3 * sizeof (long double));
    }
  memcpy (r0, msr0, 3 * sizeof (long double));
  memcpy (r1, msr1, 3 * sizeof (long double));
  equation_acceleration (eq, r0, r1, r2, t + dt);
#if DEBUG_MULTI_STEPS
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r0[0][%u]=%Lg\n", i, r0[i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r1[0][%u]=%Lg\n", i, r1[i]);
  fprintf (stderr, "multi_steps_step: end\n");
#endif
}

/**
 * Function to estimate the error on a multi-steps method step.
 */
static inline void
multi_steps_error (MultiSteps * ms,     ///< MultiSteps struct.
                   long double dt)      ///< time step size.
{
  long double e0[3], e1[3];
  Method *m;
  unsigned int i;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_error: start\n");
#endif
  m = MULTI_STEPS_METHOD (ms);
  e0[0] = e0[1] = e0[2] = e1[0] = e1[1] = e1[2] = 0.L;
  for (i = 0; i < m->nsteps; ++i)
    {
      e0[0] += ms->ea[i] * m->r0[i][0] + dt * ms->eb[i] * m->r1[i][0];
      e0[1] += ms->ea[i] * m->r0[i][1] + dt * ms->eb[i] * m->r1[i][1];
      e0[2] += ms->ea[i] * m->r0[i][2] + dt * ms->eb[i] * m->r1[i][2];
      e1[0] += ms->ea[i] * m->r1[i][0] + dt * ms->eb[i] * m->r2[i][0];
      e1[1] += ms->ea[i] * m->r1[i][1] + dt * ms->eb[i] * m->r2[i][1];
      e1[2] += ms->ea[i] * m->r1[i][2] + dt * ms->eb[i] * m->r2[i][2];
    }
  m->e0 = sqrtl (e0[0] * e0[0] + e0[1] * e0[1] + e0[2] * e0[2]);
  m->e1 = sqrtl (e1[0] * e1[0] + e1[1] * e1[1] + e1[2] * e1[2]);
  m->et0 += m->e0;
  m->et1 += m->e1;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_error: e0=%Lg et0=%Lg\n", m->e0, m->et0);
  fprintf (stderr, "multi_steps_error: e1=%Lg et1=%Lg\n", m->e1, m->et1);
  fprintf (stderr, "multi_steps_error: end\n");
#endif
}

/**
 * Function to run the multi-steps method bucle.
 *
 * \return final time.
 */
long double
multi_steps_run (MultiSteps * ms,       ///< MultiSteps struct.
                 Equation * eq) ///< Equation struct.
{
  RungeKutta *rk;
  Method *m, *mrk;
  long double t, dt, to, dto, et0o, et1o;
  unsigned int i, n;

#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_run: start\n");
#endif

  // variables backup
  rk = MULTI_STEPS_RUNGE_KUTTA (ms);
  mrk = RUNGE_KUTTA_METHOD (rk);
  m = MULTI_STEPS_METHOD (ms);
  memcpy (ro0, r0, 3 * sizeof (long double));
  memcpy (ro1, r1, 3 * sizeof (long double));
  memcpy (ro2, r2, 3 * sizeof (long double));

  // Runge-Kutta first steps
  n = m->nsteps;
  for (t = 0.L, i = n; --i > 0;)
    {

      // time step size
      if (t > 0.L && mrk->error_dt)
        {
          dto = dt;
          dt = method_dt (mrk, dt);

          // revert the step if big error
          if (dt < mrk->beta * dto)
            {
              ++i;
              t = to;
              mrk->et0 = et0o;
              mrk->et1 = et1o;
              memcpy (r0, ro0, 3 * sizeof (long double));
              memcpy (r1, ro1, 3 * sizeof (long double));
              memcpy (r2, ro2, 3 * sizeof (long double));
            }
        }
      else
        dt = equation_step_size (eq);

      // checking trajectory end
      to = t;
      if (equation_land (eq, to, &t, &dt))
        goto end;
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_run: t=%Lg dt=%Lg\n", t, dt);
#endif

      // saving step
      memcpy (m->r0[i], r0, 3 * sizeof (long double));
      memcpy (m->r1[i], r1, 3 * sizeof (long double));
      memcpy (m->r2[i], r2, 3 * sizeof (long double));
      memcpy (ro0, r0, 3 * sizeof (long double));
      memcpy (ro1, r1, 3 * sizeof (long double));
      memcpy (ro2, r2, 3 * sizeof (long double));

      // Runge-Kutta step
      runge_kutta_step (rk, eq, to, dt);

      // error estimate
      if (mrk->error_dt)
        {
          et0o = mrk->et0;
          et1o = mrk->et1;
          runge_kutta_error (rk, dt);
        }
    }

  // saving last step
  memcpy (m->r0[0], r0, 3 * sizeof (long double));
  memcpy (m->r1[0], r1, 3 * sizeof (long double));
  memcpy (m->r2[0], r2, 3 * sizeof (long double));

  // initing errors
  m->et0 = mrk->et0;
  m->et1 = mrk->et1;

  // temporal bucle
  while (1)
    {

      // time step size
      if (m->error_dt)
        {
          dto = dt;
          dt = method_dt (m, dt);

          // revert the step if big error
          if (dt < mrk->beta * dto)
            {
              ++i;
              t = to;
              m->et0 = et0o;
              m->et1 = et1o;
              memcpy (r0, ro0, 3 * sizeof (long double));
              memcpy (r1, ro1, 3 * sizeof (long double));
              memcpy (r2, ro2, 3 * sizeof (long double));
            }
        }
      else
        dt = equation_step_size (eq);

      // checking trajectory end
      to = t;
      dto = dt;
      if (equation_land (eq, to, &t, &dt))
        break;
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_run: t=%Lg dt=%Lg\n", t, dt);
#endif

      // variables backup
      memcpy (ro0, r0, 3 * sizeof (long double));
      memcpy (ro1, r1, 3 * sizeof (long double));
      memcpy (ro2, r2, 3 * sizeof (long double));

      // multi-steps step
      if (dto == dt)
        multi_steps_step (ms, eq, to, dt);
      else
        runge_kutta_step (rk, eq, to, dt);

      // error estimate
      if (m->error_dt)
        {
          et0o = m->et0;
          et1o = m->et1;
          multi_steps_error (ms, dt);
        }
    }
end:
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_run: end\n");
#endif
  return t;
}

/**
 * Function to free the memory used by a MultiSteps struct.
 */
void
multi_steps_delete (MultiSteps * ms)    ///< RungeKutta struct.
{
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_delete: start\n");
#endif
  runge_kutta_delete (MULTI_STEPS_RUNGE_KUTTA (ms));
  method_delete (MULTI_STEPS_METHOD (ms));
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_delete: end\n");
#endif
}

/**
 * Function to read on a XML node the multi-steps method input data.
 *
 * \return 1 on success, 0 on error.
 */
int
multi_steps_read_xml (MultiSteps * ms,  ///< MultiSteps struct.
                      xmlNode * node)   ///< XML node.
{
	char *message[] = {
		NULL,
		"Bad steps number",
		"Bad numerical method data",
		"No Runge-Kutta XML node",
		"Bad Runge-Kutta XML node",
		"Bad Runge-Kutta method data",
		"Bad multi-steps data"
	};
  int e, error_code;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_read_xml: start\n");
#endif
  ms->type = xml_node_get_uint (node, XML_TYPE, &error_code);
  if (error_code || !ms->type)
    e = 1;
	else if (!method_read_xml (MULTI_STEPS_METHOD (ms), node))
    e = 2;
  else
	  {
			node = node->children;
			if (!node)
				e = 3;
			else if (xmlStrcmp (node->name, XML_RUNGE_KUTTA))
				e = 4;
			else if (!runge_kutta_read_xml (MULTI_STEPS_RUNGE_KUTTA (ms), node))
				e = 5;
			else if (!multi_steps_init (ms))
				e = 6;
			else
				e = 0;
		}
	if (e)
	{
		error_add (message[e]);
		e = 0;
	}
	else
		e = 1;
#if DEBUG_MULTI_STEPS
  if (e)
    fprintf (stderr, "multi_steps_read: success\n");
  else
    fprintf (stderr, "multi_steps_read: error\n");
  fprintf (stderr, "multi_steps_read: end\n");
#endif
  return e;
}

max_stars_repo_name: jburguete/ballistic

max_issues_repo_name: jburguete/ballistic

max_forks_repo_name: jburguete/ballistic


text:
/*
 * applyForceToolFrame.h
 *
 *  Created on: Sep 11, 2015
 *      Author: mas
 */
#ifndef TANGENTIAL_VELOCITY_H_
#define TANGENTIAL_VELOCITY_H_
#include <cmath>
#include <barrett/units.h>
#include <barrett/systems.h>
#include <barrett/products/product_manager.h>
#include <libconfig.h++>
#include <barrett/detail/ca_macro.h>
#include <barrett/cdlbt/calgrav.h>
#include <barrett/systems/abstract/system.h>
#include <barrett/systems/abstract/single_io.h>
#include <barrett/systems/kinematics_base.h>
#include <barrett/math/kinematics.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <cstdlib>  // For std::atexit()
#include <barrett/os.h>  // For btsleep()
#include <barrett/math.h>  // For barrett::math::saturate()
#include <Eigen/Dense>
#include <Eigen/Core>
#include <barrett/standard_main_function.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#define BARRETT_SMF_VALIDATE_ARGS
#include <barrett/standard_main_function.h>

using namespace barrett;
using namespace systems;
using systems::connect;
BARRETT_UNITS_FIXED_SIZE_TYPEDEFS;

// --------- tangential velocity controller --------
// inputs: v = wam.toolVelocity, output: tangential velocity Vt =  dotproduct (v , tanDir)
template<size_t DOF>
class tangentialVelocityCompute : public systems::System
{
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

// IO  (inputs)
public:
Input<cv_type> toolVelocityInput;
Input<cp_type> tangentDirInput;

// IO  (outputs)
public:
Output<double> tangentVelOutput;     // tangential velocity

protected:
typename Output<double>::Value* tangentVelOutputValue;

public:
double cpomputedTangentialVel; 	 // can be used to display later in the program

public:
tangentialVelocityCompute(
      const std::string& sysName = "tangentialVelocityCompute") :
      System(sysName), toolVelocityInput(this), tangentDirInput(this),
      tangentVelOutput(this, &tangentVelOutputValue)
{
}
virtual ~tangentialVelocityCompute()
{
this->mandatoryCleanUp();
}
protected:
    cp_type tanDir;
    cv_type vel;

    double tanVel;

virtual void operate()
{

  tanDir = tangentDirInput.getValue();
  vel = toolVelocityInput.getValue();

  tanVel = tanDir.dot(vel) ;
  cpomputedTangentialVel = tanVel;

  tangentVelOutputValue->setData( & tanVel );

}

private:
DISALLOW_COPY_AND_ASSIGN(tangentialVelocityCompute);
};
// -----------------------------------

#endif /*TANGENTIAL_VELOCITY_H_*/


text:
/* linalg/qrpt.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>

#include <gsl/gsl_linalg.h>

#define REAL double

#include "givens.c"
#include "apply_givens.c"

/* Factorise a general M x N matrix A into
 *
 *   A P = Q R
 *
 * where Q is orthogonal (M x M) and R is upper triangular (M x N).
 * When A is rank deficient, r = rank(A) < n, then the permutation is
 * used to ensure that the lower n - r rows of R are zero and the first
 * r columns of Q form an orthonormal basis for A.
 *
 * Q is stored as a packed set of Householder transformations in the
 * strict lower triangular part of the input matrix.
 *
 * R is stored in the diagonal and upper triangle of the input matrix.
 *
 * P: column j of P is column k of the identity matrix, where k =
 * permutation->data[j]
 *
 * The full matrix for Q can be obtained as the product
 *
 *       Q = Q_k .. Q_2 Q_1
 *
 * where k = MIN(M,N) and
 *
 *       Q_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 *
 *       v_i = [1, m(i+1,i), m(i+2,i), ... , m(M,i)]
 *
 * This storage scheme is the same as in LAPACK.  See LAPACK's
 * dgeqpf.f for details.
 *
 */

int
gsl_linalg_QRPT_decomp (gsl_matrix * A, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (tau->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (p->size != N)
    {
      GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
    }
  else if (norm->size != N)
    {
      GSL_ERROR ("norm size must be N", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      *signum = 1;

      gsl_permutation_init (p); /* set to identity */

      /* Compute column norms and store in workspace */

      for (i = 0; i < N; i++)
        {
          gsl_vector_view c = gsl_matrix_column (A, i);
          double x = gsl_blas_dnrm2 (&c.vector);
          gsl_vector_set (norm, i, x);
        }

      for (i = 0; i < GSL_MIN (M, N); i++)
        {
          /* Bring the column of largest norm into the pivot position */

          double max_norm = gsl_vector_get(norm, i);
          size_t j, kmax = i;

          for (j = i + 1; j < N; j++)
            {
              double x = gsl_vector_get (norm, j);

              if (x > max_norm)
                {
                  max_norm = x;
                  kmax = j;
                }
            }

          if (kmax != i)
            {
              gsl_matrix_swap_columns (A, i, kmax);
              gsl_permutation_swap (p, i, kmax);
              gsl_vector_swap_elements(norm,i,kmax);

              (*signum) = -(*signum);
            }

          /* Compute the Householder transformation to reduce the j-th
             column of the matrix to a multiple of the j-th unit vector */

          {
            gsl_vector_view c_full = gsl_matrix_column (A, i);
            gsl_vector_view c = gsl_vector_subvector (&c_full.vector,
                                                      i, M - i);
            double tau_i = gsl_linalg_householder_transform (&c.vector);

            gsl_vector_set (tau, i, tau_i);

            /* Apply the transformation to the remaining columns */

            if (i + 1 < N)
              {
                gsl_matrix_view m = gsl_matrix_submatrix (A, i, i + 1, M - i, N - (i+1));

                gsl_linalg_householder_hm (tau_i, &c.vector, &m.matrix);
              }
          }

          /* Update the norms of the remaining columns too */

          if (i + 1 < M)
            {
              for (j = i + 1; j < N; j++)
                {
                  double x = gsl_vector_get (norm, j);

                  if (x > 0.0)
                    {
                      double y = 0;
                      double temp= gsl_matrix_get (A, i, j) / x;

                      if (fabs (temp) >= 1)
                        y = 0.0;
                      else
                        y = x * sqrt (1 - temp * temp);

                      /* recompute norm to prevent loss of accuracy */

                      if (fabs (y / x) < sqrt (20.0) * GSL_SQRT_DBL_EPSILON)
                        {
                          gsl_vector_view c_full = gsl_matrix_column (A, j);
                          gsl_vector_view c =
                            gsl_vector_subvector(&c_full.vector,
                                                 i+1, M - (i+1));
                          y = gsl_blas_dnrm2 (&c.vector);
                        }

                      gsl_vector_set (norm, j, y);
                    }
                }
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_decomp2 (const gsl_matrix * A, gsl_matrix * q, gsl_matrix * r, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (q->size1 != M || q->size2 !=M)
    {
      GSL_ERROR ("q must be M x M", GSL_EBADLEN);
    }
  else if (r->size1 != M || r->size2 !=N)
    {
      GSL_ERROR ("r must be M x N", GSL_EBADLEN);
    }
  else if (tau->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (p->size != N)
    {
      GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
    }
  else if (norm->size != N)
    {
      GSL_ERROR ("norm size must be N", GSL_EBADLEN);
    }

  gsl_matrix_memcpy (r, A);

  gsl_linalg_QRPT_decomp (r, tau, p, signum, norm);

  /* FIXME:  aliased arguments depends on behavior of unpack routine! */

  gsl_linalg_QR_unpack (r, tau, q, r);

  return GSL_SUCCESS;
}

/* Solves the system A x = b using the Q R P^T factorisation,

   R z = Q^T b

   x = P z;

   to obtain x. Based on SLATEC code. */

int
gsl_linalg_QRPT_solve (const gsl_matrix * QR,
                       const gsl_vector * tau,
                       const gsl_permutation * p,
                       const gsl_vector * b,
                       gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != p->size)
    {
      GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
    }
  else if (QR->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      gsl_vector_memcpy (x, b);

      gsl_linalg_QRPT_svx (QR, tau, p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_svx (const gsl_matrix * QR,
                     const gsl_vector * tau,
                     const gsl_permutation * p,
                     gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != p->size)
    {
      GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* compute sol = Q^T b */

      gsl_linalg_QR_QTvec (QR, tau, x);

      /* Solve R x = sol, storing x inplace in sol */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_QRsolve (const gsl_matrix * Q, const gsl_matrix * R,
                         const gsl_permutation * p,
                         const gsl_vector * b,
                         gsl_vector * x)
{
  if (Q->size1 != Q->size2 || R->size1 != R->size2)
    {
      return GSL_ENOTSQR;
    }
  else if (Q->size1 != p->size || Q->size1 != R->size1
           || Q->size1 != b->size)
    {
      return GSL_EBADLEN;
    }
  else
    {
      /* compute b' = Q^T b */

      gsl_blas_dgemv (CblasTrans, 1.0, Q, b, 0.0, x);

      /* Solve R x = b', storing x inplace */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, R, x);

      /* Apply permutation to solution in place */

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_Rsolve (const gsl_matrix * QR,
                        const gsl_permutation * p,
                        const gsl_vector * b,
                        gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
    }
  else if (p->size != x->size)
    {
      GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
    }
  else
    {
      /* Copy x <- b */

      gsl_vector_memcpy (x, b);

      /* Solve R x = b, storing x inplace */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_Rsvx (const gsl_matrix * QR,
                      const gsl_permutation * p,
                      gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
    }
  else if (p->size != x->size)
    {
      GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
    }
  else
    {
      /* Solve R x = b, storing x inplace */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

/* Update a Q R P^T factorisation for A P= Q R ,  A' = A + u v^T,

   Q' R' P^-1 = QR P^-1 + u v^T
   = Q (R + Q^T u v^T P ) P^-1
   = Q (R + w v^T P) P^-1

   where w = Q^T u.

   Algorithm from Golub and Van Loan, "Matrix Computations", Section
   12.5 (Updating Matrix Factorizations, Rank-One Changes)  */

int
gsl_linalg_QRPT_update (gsl_matrix * Q, gsl_matrix * R,
                        const gsl_permutation * p,
                        gsl_vector * w, const gsl_vector * v)
{
  if (Q->size1 != Q->size2 || R->size1 != R->size2)
    {
      return GSL_ENOTSQR;
    }
  else if (R->size1 != Q->size2 || v->size != Q->size2 || w->size != Q->size2)
    {
      return GSL_EBADLEN;
    }
  else
    {
      size_t j, k;
      const size_t M = Q->size1;
      const size_t N = Q->size2;
      double w0;

      /* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)

         J_1^T .... J_(n-1)^T w = +/- |w| e_1

         simultaneously applied to R,  H = J_1^T ... J^T_(n-1) R
         so that H is upper Hessenberg.  (12.5.2) */

      for (k = N - 1; k > 0; k--)
        {
          double c, s;
          double wk = gsl_vector_get (w, k);
          double wkm1 = gsl_vector_get (w, k - 1);

          create_givens (wkm1, wk, &c, &s);
          apply_givens_vec (w, k - 1, k, c, s);
          apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
        }

      w0 = gsl_vector_get (w, 0);

      /* Add in w v^T  (Equation 12.5.3) */

      for (j = 0; j < N; j++)
        {
          double r0j = gsl_matrix_get (R, 0, j);
          size_t p_j = gsl_permutation_get (p, j);
          double vj = gsl_vector_get (v, p_j);
          gsl_matrix_set (R, 0, j, r0j + w0 * vj);
        }

      /* Apply Givens transformations R' = G_(n-1)^T ... G_1^T H
         Equation 12.5.4 */

      for (k = 1; k < N; k++)
        {
          double c, s;
          double diag = gsl_matrix_get (R, k - 1, k - 1);
          double offdiag = gsl_matrix_get (R, k, k - 1);

          create_givens (diag, offdiag, &c, &s);
          apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
        }

      return GSL_SUCCESS;
    }
}

max_stars_repo_name: manggoguy/parsec-modified

max_issues_repo_name: manggoguy/parsec-modified

max_forks_repo_name: manggoguy/parsec-modified


text:
#pragma once

#include <functional>
#include <gsl/gsl>
#include <memory>
#include <vector>
#include "halley/text/halleystring.h"
#include "halley/maths/vector2.h"
#include "halley/maths/vector3.h"
#include <memory>

namespace Halley
{
	class AudioPosition;
	class AudioClip;
	class AudioEvent;
	class IAudioClip;
	class AudioEmitterBehaviour;

    namespace AudioConfig {
        constexpr int sampleRate = 48000;
		constexpr int maxChannels = 8;
        using SampleFormat = float;
    }

	class AudioDevice
	{
	public:
		virtual ~AudioDevice() {}
		virtual String getName() const = 0;
	};

	struct alignas(64) AudioSamplePack
	{
		constexpr static int NumSamples = 16;
		std::array<AudioConfig::SampleFormat, NumSamples> samples; // AVX-512 friendly
	};

	enum class AudioSampleFormat
	{
		Undefined,
		Int16,
		Int32,
		Float
	};

	template <>
	struct EnumNames<AudioSampleFormat> {
		constexpr std::array<const char*, 4> operator()() const {
			return{{
				"undefined",
				"int16",
				"int32",
				"float"
			}};
		}
	};

	class AudioSpec
	{
	public:
		int sampleRate;
		int numChannels;
		int bufferSize;
		AudioSampleFormat format;

		AudioSpec() {}
		AudioSpec(int sampleRate, int numChannels, int bufferSize, AudioSampleFormat format)
			: sampleRate(sampleRate)
			, numChannels(numChannels)
			, bufferSize(bufferSize)
			, format(format)
		{}
	};

	class AudioListenerData
	{
	public:
		Vector3f position;

		AudioListenerData() {}
		AudioListenerData(Vector3f position)
			: position(position)
		{}
	};

	using AudioCallback = std::function<void()>;

	class AudioOutputAPI
	{
	public:
		virtual ~AudioOutputAPI() {}

		virtual Vector<std::unique_ptr<const AudioDevice>> getAudioDevices() = 0;
		virtual AudioSpec openAudioDevice(const AudioSpec& requestedFormat, const AudioDevice* device = nullptr, AudioCallback prepareAudioCallback = AudioCallback()) = 0;
		virtual void closeAudioDevice() = 0;

		virtual void startPlayback() = 0;
		virtual void stopPlayback() = 0;

		virtual void queueAudio(gsl::span<const float> data) = 0;
		virtual bool needsMoreAudio() = 0;

		virtual bool needsAudioThread() const = 0;
	};

	class IAudioHandle
	{
	public:
		virtual ~IAudioHandle() {}

		virtual void setGain(float gain) = 0;
		virtual void setVolume(float volume) = 0;
		virtual void setPosition(Vector2f pos) = 0;
		virtual void setPan(float pan) = 0;

		virtual void stop(float fadeTime = 0.0f) = 0;
		virtual bool isPlaying() const = 0;
		virtual void setBehaviour(std::unique_ptr<AudioEmitterBehaviour> behaviour) = 0;
	};
	using AudioHandle = std::shared_ptr<IAudioHandle>;

	class AudioAPI
	{
	public:
		virtual ~AudioAPI() {}

		virtual Vector<std::unique_ptr<const AudioDevice>> getAudioDevices() = 0;
		virtual void startPlayback(int deviceNumber = 0) = 0;
		virtual void stopPlayback() = 0;
		virtual void pausePlayback() = 0;
		virtual void resumePlayback() = 0;

		virtual AudioHandle postEvent(const String& name, AudioPosition position) = 0;

		virtual AudioHandle play(std::shared_ptr<const IAudioClip> clip, AudioPosition position, float volume = 1.0f, bool loop = false) = 0;
		virtual AudioHandle playMusic(const String& eventName, int track = 0) = 0;
		virtual AudioHandle getMusic(int track = 0) = 0;
		virtual void stopMusic(int track = 0, float fadeOutTime = 0.0f) = 0;
		virtual void stopAllMusic(float fadeOutTime = 0.0f) = 0;

		virtual void setMasterVolume(float gain = 1.0f) = 0;
		virtual void setGroupVolume(const String& groupName, float gain = 1.0f) = 0;

		virtual void setListener(AudioListenerData listener) = 0;
	};
}

max_stars_repo_name: Healthire/halley

max_issues_repo_name: Healthire/halley

max_forks_repo_name: Healthire/halley