void
gameinfo_collision_ball_slime(GameInfo *gi, TEAM t) {
Ball *b = gi->ball;
Slime *s;
if (t == BLUE ) {
s = gi->slime_blue;
} else {
s = gi->slime_red;
}
Vector v_s = {b->vx, b->vy};
Vector v_a = {s->x - b->x, WIN_HEIGHT-b->y};
Vector v_b = {s->x - (b->x + b->vx), WIN_HEIGHT - (b->y + b->vy)};
double c = vector_inner_product (v_a, v_s) *
vector_inner_product (v_b, v_s) ;
double d = fabs(vector_outer_product (v_s, v_a))/ vector_norm(v_s);
double r = SLIME_RADIUS + BALL_RADIUS;
if ( d <= r && ( c<=0 || r > vector_norm (v_a) || r > vector_norm (v_b))) {
double dx_0 = b->x - s->x;
double dy_0 = b->y - WIN_HEIGHT;
double dist_0 = sqrt(pow(dx_0, 2) + pow(dy_0, 2));
double angle = acos (dx_0/dist_0);
double cos2x = cos(2*angle);
double sin2x = sin(2*angle);
double vx = - b->vx * cos2x + b->vy * sin2x;
double vy = b->vx * sin2x + b->vy * cos2x;
b->vx = vx;
b->vy = vy;
#if 0
double pe = GRAVITY * (WIN_HEIGHT - b->y );
double ke = 0.5 * (pow(b->vx, 2) + pow(b->vy, 2));
pe = GRAVITY * (WIN_HEIGHT - b->y );
ke = 0.5 * (pow(b->vx, 2) + pow(b->vy, 2));
g_print("POST x=%+.2f vx=%+.2f y=%+.2f vy=%+.2f pe=%+.4f ke=%+.4f pe+ke=%+.4f\n",
b->x, b->vx, b->y, b->vy, pe, ke, pe+ke);
#endif
gi->ball_owner = t;
gi->ball_count++;
}
}
void eta_file_btran(eta_file* ef, EXLPvector* c, EXLPvector* y) {
/* yB = c を解く. */
eta_matrix* U;
mpq_t q;
int i;
U = my_malloc(sizeof(eta_matrix));
vector_copy(y, c);
mpq_init(q);
vector_permutate(ef->R, y);
for (i = 0; i < ef->U->columns; i ++) {
U->eta_column = i;
U->eta_vector = ef->U->column[i];
eta_matrix_solve_yE_is_equal_to_v(U, y);
}
vector_permutate(ef->Q, y);
for (i = ef->s-1; i >= 0; i --) {
if (ef->Ls[i]->row == ef->Ls[i]->column) {
vector_mul_element(y, ef->Ls[i]->value, ef->Ls[i]->row);
} else {
vector_get_element(&q, y, ef->Ls[i]->row);
mympq_mul(q, q, ef->Ls[i]->value);
vector_add_element(y, q, ef->Ls[i]->column);
}
}
for (i = ef->U->columns-1; i >= 0; i --) {
if (ef->L[i]->eta_vector->nonzeros == 1) {
vector_mul_element(y, ef->L[i]->eta_vector->value[0], i);
} else {
vector_inner_product(&q, y, ef->L[i]->eta_vector);
vector_set_element(y, q, i);
}
vector_swap_elements(y, i, ef->P[i]);
}
mpq_clear(q);
free(U);
}
void vector_normalize(double *x, int n) {
vector_scale(x, 1 / sqrt(vector_inner_product(x, x, n)), n);
}
static VALUE vector_magnitude(VALUE self) {
double mag = sqrt(NUM2DBL(vector_inner_product(self, self)));
return rb_float_new(mag);
}
void lp_sol_init_dual(LP* lp) {
mpq_t q1, q2;
mpq_t* q3;
EXLPvector* v1;
EXLPvector* v2;
int i, j, k;
if (lp->preprocess < 2) {
mpq_init(q1);
//mpq_set_si(q1, 1, 10000);
for (i = 0; i < lp->vars; i ++) {
if (is_const_var(lp, i))
continue;
mpq_set_si(q1, i, lp->vars*lp->vars*lp->vars*lp->vars);
if (lp->lower.is_valid[i] && lp->upper.is_valid[i]) {
q3 = vector_get_element_ptr(lp->c, i);
if (mpq_sgn(*q3) > 0) {
vector_set_element(lp->x, lp->upper.bound[i], i);
if (lp->is_basis[i]) {
vector_sub_element(lp->x, q1, i);
if (mpq_cmp(*vector_get_element_ptr(lp->x, i),
lp->lower.bound[i]) < 0)
vector_add_element(lp->x, q1, i);
}
} else {
vector_set_element(lp->x, lp->lower.bound[i], i);
if (lp->is_basis[i]) {
vector_add_element(lp->x, q1, i);
if (mpq_cmp(*vector_get_element_ptr(lp->x, i),
lp->upper.bound[i]) > 0)
vector_sub_element(lp->x, q1, i);
}
}
} if (lp->lower.is_valid[i]) {
vector_set_element(lp->x, lp->lower.bound[i], i);
if (lp->is_basis[i])
vector_add_element(lp->x, q1, i);
} else if (lp->upper.is_valid[i]) {
vector_set_element(lp->x, lp->upper.bound[i], i);
if (lp->is_basis[i])
vector_sub_element(lp->x, q1, i);
}
}
mpq_clear(q1);
for (i = 0; i < lp->rows; i ++) {
vector_set_element(lp->xb, *vector_get_element_ptr(lp->x, lp->basis_column[i]), i);
}
} else {
reinversion(lp);
mpq_init(q1);
mpq_init(q2);
v1 = new_vector(lp->rows);
v2 = new_vector(lp->vars);
eta_file_btran(lp->eta, lp->cb, v1);
vector_copy(v2, lp->c);
for (i = 0; i < lp->vars; i ++) {
if (lp->is_basis[i] || is_const_var(lp, i))
continue;
vector_inner_product(&q1, v1, lp->A->column[i]);
vector_sub_element(v2, q1, i);
}
for (i = 0; i < lp->vars; i ++) {
if (is_const_var(lp, i) || lp->is_basis[i])
continue;
if (lp->lower.is_valid[i] && lp->upper.is_valid[i]) {
q3 = vector_get_element_ptr(v2, i);
if (mpq_sgn(*q3) > 0)
vector_set_element(lp->x, lp->upper.bound[i], i);
else
vector_set_element(lp->x, lp->lower.bound[i], i);
} else if (lp->lower.is_valid[i]) {
vector_set_element(lp->x, lp->lower.bound[i], i);
} else if (lp->upper.is_valid[i]) {
vector_set_element(lp->x, lp->upper.bound[i], i);
}
}
vector_resize(v2, lp->rows);
vector_copy(v1, lp->b);
for (i = 0; i < lp->rows; i ++) {
mpq_set_si(q1, 0, 1);
for (j = 0; j < lp->A->row[i]->nonzeros; j ++) {
k = lp->A->row[i]->i[j];
if (is_const_var(lp, k) || lp->is_basis[k])
continue;
mympq_mul(q2, *matrix_get_element_ptr(lp->A, i, k),
*vector_get_element_ptr(lp->x, k));
mympq_add(q1, q1, q2);
}
vector_sub_element(v1, q1, i);
}
eta_file_ftran(lp->eta, v1, v2, NULL);
//mpq_set_si(q1, 1, 10000);
mpq_set_si(q1, i, lp->vars*lp->vars*lp->vars*lp->vars);
for (i = 0; i < lp->rows; i ++) {
k = lp->basis_column[i];
q3 = vector_get_element_ptr(v2, i);
//mpq_set_si(q1, 1, 100000);mympq_mul(q1, q1, *q3);
if (lp->lower.is_valid[k] &&
mpq_cmp(*q3, lp->lower.bound[k]) <= 0) {
vector_set_element(lp->x, lp->lower.bound[k], k);
vector_set_element(lp->xb, lp->lower.bound[k], i);
vector_add_element(lp->x, q1, k);
vector_add_element(lp->xb, q1, i);
if (lp->upper.is_valid[k] &&
mpq_cmp(lp->upper.bound[k],
*vector_get_element_ptr(lp->x, k)) < 0) {
vector_sub_element(lp->x, q1, k);
vector_sub_element(lp->xb, q1, i);
}
continue;
}
if (lp->upper.is_valid[k] &&
mpq_cmp(*q3, lp->upper.bound[k]) >= 0) {
vector_set_element(lp->x, lp->upper.bound[k], k);
vector_set_element(lp->xb, lp->upper.bound[k], i);
vector_sub_element(lp->x, q1, k);
vector_sub_element(lp->xb, q1, i);
if (lp->lower.is_valid[k] &&
mpq_cmp(lp->lower.bound[k],
*vector_get_element_ptr(lp->x, k)) > 0) {
vector_add_element(lp->x, q1, k);
vector_add_element(lp->xb, q1, i);
}
continue;
}
vector_set_element(lp->x, *q3, k);
vector_set_element(lp->xb, *q3, i);
}
mpq_clear(q1);
mpq_clear(q2);
vector_free(&v1);
vector_free(&v2);
}
}
void lp_get_object_value(LP* lp, mpq_t* q) {
vector_inner_product(q, lp->c_back, lp->x);
mympq_add(*q, *q, lp->c_const);
if (lp->maximize == FALSE)
mpq_neg(*q, *q);
}
