inline fvar<T>
trace_gen_quad_form(const Eigen::Matrix<fvar<T>, RD, CD> &D,
const Eigen::Matrix<fvar<T>, RA, CA> &A,
const Eigen::Matrix<fvar<T>, RB, CB> &B) {
check_square("trace_gen_quad_form", "A", A);
check_square("trace_gen_quad_form", "D", D);
check_multiplicable("trace_gen_quad_form", "A", A, "B", B);
check_multiplicable("trace_gen_quad_form", "B", B, "D", D);
return trace(multiply(multiply(D, transpose(B)),
multiply(A, B)));
}
inline double
trace_gen_quad_form(const Eigen::Matrix<double, RD, CD> &D,
const Eigen::Matrix<double, RA, CA> &A,
const Eigen::Matrix<double, RB, CB> &B) {
check_square("trace_gen_quad_form", "A", A);
check_square("trace_gen_quad_form", "D", D);
check_multiplicable("trace_gen_quad_form",
"A", A,
"B", B);
check_multiplicable("trace_gen_quad_form",
"B", B,
"D", D);
return (D*B.transpose()*A*B).trace();
}
inline
Eigen::Matrix<fvar<T>, R1, C2>
mdivide_right_tri_low(const Eigen::Matrix<fvar<T>, R1, C1> &A,
const Eigen::Matrix<double, R2, C2> &b) {
check_square("mdivide_right_tri_low", "b", b);
check_multiplicable("mdivide_right_tri_low", "A", A, "b", b);
Eigen::Matrix<T, R2, C2> deriv_b_mult_inv_b(b.rows(), b.cols());
Eigen::Matrix<T, R1, C1> val_A(A.rows(), A.cols());
Eigen::Matrix<T, R1, C1> deriv_A(A.rows(), A.cols());
Eigen::Matrix<T, R2, C2> val_b(b.rows(), b.cols());
val_b.setZero();
for (int j = 0; j < A.cols(); j++) {
for (int i = 0; i < A.rows(); i++) {
val_A(i, j) = A(i, j).val_;
deriv_A(i, j) = A(i, j).d_;
}
}
for (size_type j = 0; j < b.cols(); j++) {
for (size_type i = j; i < b.rows(); i++) {
val_b(i, j) = b(i, j);
}
}
return to_fvar(mdivide_right(val_A, val_b),
mdivide_right(deriv_A, val_b));
}
inline
Eigen::Matrix<var, R1, C2>
mdivide_left_spd(const Eigen::Matrix<double, R1, C1> &A,
const Eigen::Matrix<var, R2, C2> &b) {
Eigen::Matrix<var, R1, C2> res(b.rows(), b.cols());
check_square("mdivide_left_spd", "A", A);
check_multiplicable("mdivide_left_spd",
"A", A,
"b", b);
// NOTE: this is not a memory leak, this vari is used in the
// expression graph to evaluate the adjoint, but is not needed
// for the returned matrix. Memory will be cleaned up with the
// arena allocator.
mdivide_left_spd_dv_vari<R1, C1, R2, C2> *baseVari
= new mdivide_left_spd_dv_vari<R1, C1, R2, C2>(A, b);
size_t pos = 0;
for (size_type j = 0; j < res.cols(); j++)
for (size_type i = 0; i < res.rows(); i++)
res(i, j).vi_ = baseVari->variRefC_[pos++];
return res;
}
int check_all(int x, int y, int value, char **map)
{
if (check_col(x, value, map) == 1 && check_line(y, value, map) == 1 &&
check_square(x, y, value, map) == 1)
return (1);
return (0);
}
int validate(grid board, int possible, int i, int j) {
int valid_rows = check_row(board, possible, i, j);
int valid_cols = check_column(board, possible, i, j);
int valid_square = check_square(board, possible, i, j);
return valid_rows && valid_cols && valid_square;
}
void
draw_numbers(void)
{
// iterate over board's numbers
for (int i = 0; i < 9; i++)
{
for (int j = 0; j < 9; j++)
{
if (has_colors() && g.board[i][j] != 0 && g.board[i][j] == g.init_board[i][j])
attron(COLOR_PAIR(PAIR_INIT));
if( has_colors() && (!check_col(j) || !check_row(i) || !check_square(i,j)))
attron(COLOR_PAIR(PAIR_ERR));
if (game_won() && has_colors())
attron(COLOR_PAIR(PAIR_WON));
// determine char
char c = (g.board[i][j] == 0) ? '.' : g.board[i][j] + '0';
mvaddch(g.top + i + 1 + i/3, g.left + 2 + 2*(j + j/3), c);
if (has_colors())
attroff(COLOR_PAIR(PAIR_INIT));
refresh();
}
}
}
inline
Eigen::Matrix<fvar<T>, R1, C2>
mdivide_right_tri_low(const Eigen::Matrix<double, R1, C1> &A,
const Eigen::Matrix<fvar<T>, R2, C2> &b) {
check_square("mdivide_right_tri_low", "b", b);
check_multiplicable("mdivide_right_tri_low", "A", A, "b", b);
Eigen::Matrix<T, R1, C2>
A_mult_inv_b(A.rows(), b.cols());
Eigen::Matrix<T, R2, C2> deriv_b_mult_inv_b(b.rows(), b.cols());
Eigen::Matrix<T, R2, C2> val_b(b.rows(), b.cols());
Eigen::Matrix<T, R2, C2> deriv_b(b.rows(), b.cols());
val_b.setZero();
deriv_b.setZero();
for (int j = 0; j < b.cols(); j++) {
for (int i = j; i < b.rows(); i++) {
val_b(i, j) = b(i, j).val_;
deriv_b(i, j) = b(i, j).d_;
}
}
A_mult_inv_b = mdivide_right(A, val_b);
deriv_b_mult_inv_b = mdivide_right(deriv_b, val_b);
Eigen::Matrix<T, R1, C2>
deriv(A.rows(), b.cols());
deriv = -multiply(A_mult_inv_b, deriv_b_mult_inv_b);
return to_fvar(A_mult_inv_b, deriv);
}
int mat_inverse(matrix *mat1, matrix mat2){
matrix square;
if(check_square(mat2) != 0 || check_square(*mat1) != 0){
return -1;
}
mat_alloc(&square, mat2.row, mat2.col);
mat_unit(&square);
mat_solve(mat1, mat2, square);
mat_print(square);
mat_print(mat2);
mat_print(*mat1);
return 0;
}
inline
Eigen::Matrix<T, R1, C1>
mdivide_left_tri(const Eigen::Matrix<T, R1, C1> &A) {
check_square("mdivide_left_tri", "A", A);
int n = A.rows();
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> b;
b.setIdentity(n, n);
A.template triangularView<TriView>().solveInPlace(b);
return b;
}
inline
Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
R1, C2>
mdivide_left_tri(const Eigen::Matrix<T1, R1, C1> &A,
const Eigen::Matrix<T2, R2, C2> &b) {
check_square("mdivide_left_tri", "A", A);
check_multiplicable("mdivide_left_tri", "A", A, "b", b);
return promote_common<Eigen::Matrix<T1, R1, C1>,
Eigen::Matrix<T2, R1, C1> >(A)
.template triangularView<TriView>()
.solve(promote_common<Eigen::Matrix<T1, R2, C2>,
Eigen::Matrix<T2, R2, C2> >(b));
}
// 正方行列なら単位行列を返す
int mat_unit(matrix *mat){
int i, j;
if(check_square(*mat) != 0){
printf("%s\n", "エラーが発生しました。");
return -1;
}
for(i=0; i < mat->row; i++){
for(j=0; j < mat->col; j++){
mat->element[i][j] = (i == j) ? 1 : 0;
}
}
return 0;
}
inline
Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
R1, C2>
mdivide_right(const Eigen::Matrix<T1, R1, C1> &b,
const Eigen::Matrix<T2, R2, C2> &A) {
check_square("mdivide_right", "A", A);
check_multiplicable("mdivide_right", "b", b, "A", A);
// FIXME: This is nice and general but likely slow.
return transpose(mdivide_left(transpose(A), transpose(b)));
// return promote_common<Eigen::Matrix<T1, R2, C2>,
// Eigen::Matrix<T2, R2, C2> >(A)
// .transpose()
// .lu()
// .solve(promote_common<Eigen::Matrix<T1, R1, C1>,
// Eigen::Matrix<T2, R1, C1> >(b)
// .transpose())
// .transpose();
}
int parse_number_threads(int argc, char** argv){
if (argc != 2){
printf("You didn't enter the correct number of arguments\n");
print_usage();
exit(EXIT_FAILURE);
}
int threads = atoi(argv[1]);
if (threads < 1 || !check_square(threads)){
printf("You entered an invalid number of threads\n");
printf("Please select a square integer greater than 1\n");
print_usage();
exit(EXIT_FAILURE);
}
return threads;
}
inline typename
boost::enable_if_c<!stan::is_var<T1>::value &&
!stan::is_var<T2>::value &&
!stan::is_var<T3>::value,
typename
boost::math::tools::promote_args<T1, T2, T3>::type>::type
trace_gen_inv_quad_form_ldlt(const Eigen::Matrix<T1, R1, C1> &D,
const LDLT_factor<T2, R2, C2> &A,
const Eigen::Matrix<T3, R3, C3> &B) {
check_square("trace_gen_inv_quad_form_ldlt", "D", D);
check_multiplicable("trace_gen_inv_quad_form_ldlt",
"A", A,
"B", B);
check_multiplicable("trace_gen_inv_quad_form_ldlt",
"B", B,
"D", D);
return trace(multiply(multiply(D, transpose(B)),
mdivide_left_ldlt(A, B)));
}
inline typename
boost::enable_if_c<stan::is_var<T1>::value ||
stan::is_var<T2>::value ||
stan::is_var<T3>::value, var>::type
trace_gen_inv_quad_form_ldlt(const Eigen::Matrix<T1, R1, C1> &D,
const LDLT_factor<T2, R2, C2> &A,
const Eigen::Matrix<T3, R3, C3> &B) {
check_square("trace_gen_inv_quad_form_ldlt", "D", D);
check_multiplicable("trace_gen_inv_quad_form_ldlt",
"A", A,
"B", B);
check_multiplicable("trace_gen_inv_quad_form_ldlt",
"B", B,
"D", D);
trace_inv_quad_form_ldlt_impl<T2, R2, C2, T3, R3, C3> *_impl
= new trace_inv_quad_form_ldlt_impl<T2, R2, C2, T3, R3, C3>(D, A, B);
return var(new trace_inv_quad_form_ldlt_vari<T2, R2, C2, T3, R3, C3>
(_impl));
}
inline T determinant(const Eigen::Matrix<T, R, C>& m) {
check_square("determinant", "m", m);
return m.determinant();
}
// アレする
int mat_solve(matrix *mat1, matrix mat2, matrix mat3){
int i, j, k, l;
double multiplier, divisor;
matrix a, b;
if(check_square(mat2) != 0 || mat2.row != mat3.row || mat1->row != mat3.row || mat1->col != mat3.col ){
return -1;
}
mat_alloc(&a, mat2.row, mat2.col);
mat_alloc(&b, mat3.row, mat3.col);
mat_copy(&a, mat2);
mat_copy(&b, mat3);
mat_print(a);
mat_print(b);
// Gaussの消去法
for(i=0; i<a.row; i++){
// i+1行目以降のi列目を消す（i=0のとき、2行目〜最終行の1列目を消去）
for(j=i+1; j<a.row; j++){
// j行目に、i行目を何倍したら、i列目が消えるか（例えば、i=0のとき、2行目以降の行全体に、何倍すれば、1列目が0になるか）
multiplier = a.element[j][i] / a.element[i][i];
for(k=0; k<a.col; k++){
a.element[j][k] = a.element[j][k] - a.element[i][k] * multiplier;
}
for(l=0; l<b.col; l++){
b.element[j][l] = b.element[j][l] - b.element[i][l] * multiplier;
}
}
}
// 後退代入
// 最終行以前の行に対して
for(i=a.row-1; i>=0; i--){
// 例えば、今2行目なら、3列目〜最終列に対して処理を行う
for(j=a.row-1; j>i; j--){
divisor = a.element[i][j];
a.element[i][j] = a.element[i][j] - divisor * a.element[j][j];
for(l=0; l<b.col; l++){
b.element[i][l] = b.element[i][l] - divisor * b.element[j][l];
}
}
for(l=0; l<b.col; l++){
b.element[i][l] = b.element[i][l] / a.element[i][i];
}
a.element[i][i] = 1.0;
}
mat_copy(mat1, b);
mat_free(&a);
mat_free(&b);
return 0;
}
ARMarkerInfo2*
Tracker::arDetectMarker2(int16_t *limage, int label_num, int *label_ref,
int *warea, ARFloat *wpos, int *wclip,
int area_max, int area_min, ARFloat factor, int *marker_num)
{
ARMarkerInfo2 *pm;
int xsize, ysize;
int marker_num2;
int i, j, ret;
ARFloat d;
if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) {
area_min /= 4;
area_max /= 4;
xsize = arImXsize / 2;
ysize = arImYsize / 2;
}
else {
xsize = arImXsize;
ysize = arImYsize;
}
marker_num2 = 0;
for(i=0; i<label_num; i++ ) {
if( warea[i] < area_min || warea[i] > area_max ) continue;
if( wclip[i*4+0] == 1 || wclip[i*4+1] == xsize-2 ) continue;
if( wclip[i*4+2] == 1 || wclip[i*4+3] == ysize-2 ) continue;
ret = arGetContour( limage, label_ref, i+1,
&(wclip[i*4]), &(marker_infoTWO[marker_num2]));
if( ret < 0 ) continue;
ret = check_square( warea[i], &(marker_infoTWO[marker_num2]), factor );
if( ret < 0 ) continue;
marker_infoTWO[marker_num2].area = warea[i];
marker_infoTWO[marker_num2].pos[0] = wpos[i*2+0];
marker_infoTWO[marker_num2].pos[1] = wpos[i*2+1];
marker_num2++;
if(marker_num2==MAX_IMAGE_PATTERNS)
break;
}
for( i=0; i < marker_num2; i++ ) {
for( j=i+1; j < marker_num2; j++ ) {
d = (marker_infoTWO[i].pos[0] - marker_infoTWO[j].pos[0])
* (marker_infoTWO[i].pos[0] - marker_infoTWO[j].pos[0])
+ (marker_infoTWO[i].pos[1] - marker_infoTWO[j].pos[1])
* (marker_infoTWO[i].pos[1] - marker_infoTWO[j].pos[1]);
if( marker_infoTWO[i].area > marker_infoTWO[j].area ) {
if( d < marker_infoTWO[i].area / 4 ) {
marker_infoTWO[j].area = 0;
}
}
else {
if( d < marker_infoTWO[j].area / 4 ) {
marker_infoTWO[i].area = 0;
}
}
}
}
for( i=0; i < marker_num2; i++ ) {
if( marker_infoTWO[i].area == 0.0 ) {
for( j=i+1; j < marker_num2; j++ ) {
marker_infoTWO[j-1] = marker_infoTWO[j];
}
marker_num2--;
}
}
if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) {
pm = &(marker_infoTWO[0]);
for( i = 0; i < marker_num2; i++ ) {
pm->area *= 4;
pm->pos[0] *= 2.0;
pm->pos[1] *= 2.0;
for( j = 0; j< pm->coord_num; j++ ) {
pm->x_coord[j] *= 2;
pm->y_coord[j] *= 2;
}
pm++;
}
}
*marker_num = marker_num2;
return( &(marker_infoTWO[0]) );
}
