v3_t v3_unit(const v3_t v) {
double mag = v3_mag(v);
if (mag == 0)
return v;
return v3_scale(1.0 / mag, v);
}
void v3_unit(v3* v) {
float mag = v3_mag(v);
if (mag == 0) return;
mag = 1 / mag;
v->v[0] *= mag;
v->v[1] *= mag;
v->v[2] *= mag;
return;
}
//get the angle from vector v1 to vector v2 around the axis
double GetAngle(double* v1, double* v2, double* axis)
{
double dot_prod = v3_dot(v1, v2);
double r_axis_len = v3_mag(axis);
double theta = atan2(r_axis_len, dot_prod);
return theta;
}
hit_test intersect_cylinder (ray r, cylinder c) {
hit_test result;
v3 rp = v3_expr(r.direction.x,0,r.direction.z);
double mp = v3_mag(rp);
v3 np = v3_norm(rp);
double xbar = r.origin.x - c.center.x;
double zbar = r.origin.z - c.center.z;
double a = pow(np.x,2) + pow(np.z,2);
double b = 2 * ( (xbar*np.x) + (zbar*np.z) );
double c1 = pow(xbar,2) + pow(zbar,2) - pow(c.radius,2);
double d = pow(b,2) - (4*a*c1);
result.miss = MISS;
if(d >= 0) {
double t_front = (-b - sqrt(d)) / (2*a);
double t_back = (-b + sqrt(d)) / (2*a);
v3 p_front = ray_position(r, t_front/mp);
v3 p_back = ray_position(r, t_back/mp);
if( (t_front < t_back) &&
(t_front > 0) &&
(p_front.y >= c.center.y) && (on_cylinder(p_front, c))) {
result.miss = HIT;
result.t = (t_front/mp);
result.hit_point = p_front;
result.surf = c.surface_color(c.center, p_front);
result.shine = c.shine;
v3 c2 = v3_expr(c.center.x, p_front.y, c.center.z);
result.surf_norm = v3_norm(v3_sub(p_front,c2));
} else if( (t_back > 0) &&
(p_back.y >= c.center.y) &&
(on_cylinder(p_back, c)) ) {
result.miss = HIT;
result.t = (t_back/mp);
result.hit_point = p_back;
result.surf = c.surface_color(c.center, p_back);
result.shine = c.shine;
v3 c3 = v3_expr(c.center.x, p_back.y, c.center.z);
result.surf_norm = v3_norm(v3_sub(c3,p_back));
}
}
return result;
}
/* Compute the "median" of a set of vectors */
v3_t v3_median(int n, const v3_t *v) {
int i;
v3_t median = v3_new(0.0, 0.0, 0.0);
double min_dist = DBL_MAX;
for (i = 0; i < n; i++) {
double dist = 0.0;
int j;
for (j = 0; j < n; j++) {
dist += v3_mag(v3_sub(v[j], v[i]));
}
if (dist < min_dist) {
min_dist = dist;
median = v[i];
}
}
return median;
}
void v3_rot_v3(v3* va, const v3* vr, float angle) {
float mag = v3_mag(vr);
float mag2 = mag * mag;
float x = va->v[0];
float y = va->v[1];
float z = va->v[2];
float u = vr->v[0];
float v = vr->v[1];
float w = vr->v[2];
float u2 = u * u;
float v2 = v * v;
float w2 = w * w;
float c = cos(angle);
float s = sin(angle);
float m00 = (u2 + (v2 + w2) * c) / mag2;
float m01 = (u * v * (1 - c) + w * mag * s) / mag2;
float m02 = (v * w * (1 - c) - v * mag * s) / mag2;
float m10 = (u * v * (1 - c) - w * mag * s) / mag2;
float m11 = (v2 + (u2 + w2) * c) / mag2;
float m12 = (v * w * (1 - c) + u * mag * s) / mag2;
float m20 = (u * w * (1 - c) + v * mag * s) / mag2;
float m21 = (v * w * (1 - c) - u * mag * s) / mag2;
float m22 = (w2 + (u2 + v2) * c) / mag2;
va->v[0] = m00 * x + m10 * y + m20 * z;
va->v[1] = m01 * x + m11 * y + m21 * z;
va->v[2] = m02 * x + m12 * y + m22 * z;
return;
}
GLvoid sample_world() {
// int timing;
u_int i,j,d;
// float t0,t1,t2;
v3* temp;
v3* disp;
// v3* disp2;
float dot;
float dm;
char* temp_s1;
char* temp_s2;
for (i = 0; i < PARTICLES; i++) {
// printf("adding g %d,%f\n", i,ps[i].v[1]);
v3_add(ps[i].v, gr);
// ps[i].v->v[1] -= g;
v3_mult(ps[i].v, LOSS_V);
for (d = 0; d < D; d++) {
v3_mult_add(ps[i].c, ps[i].v, 1 / (float) SPS); // ps[i].c[d] += ps[i].v[d] / (float) SPS;
if (ps[i].c->v[d] < -B + RADIUS) {
disp = v3_new(0,0,0);
disp->v[d] = ps[i].c->v[d] + B;
dm = 1 / (1 - (RADIUS - v3_mag(disp)) / RADIUS);
v3_unit(disp);
v3_mult_add(ps[i].v, disp, dm * LOSS_WALL * dt);
// ps[i].c->v[d] = -B + RADIUS;
// ps[i].v->v[d] *= -1 * LOSS_WALL;
}
if (ps[i].c->v[d] > B - RADIUS) {
disp = v3_new(0,0,0);
disp->v[d] = ps[i].c->v[d] - B;
// dm = (RADIUS - v3_mag(disp)) / RADIUS;
dm = 1 / (1 - (RADIUS - v3_mag(disp)) / RADIUS);
v3_unit(disp);
v3_mult_add(ps[i].v, disp, dm * LOSS_WALL * dt);
// ps[i].c->v[d] = B - RADIUS;
// ps[i].v->v[d] *= -1 * LOSS_WALL;
}
}
}
for (i = 0; i < PARTICLES; i++) {
for (j = 0; j < PARTICLES; j++) {
if (i == j) continue;
if (v3_dist(ps[i].c, ps[j].c) >= RADIUS * 2) continue;
/* temp_s1 = v3_str(ps[i].c);
temp_s2 = v3_str(ps[j].c);
printf("collision pos: %s %s\n", temp_s1, temp_s2);
free(temp_s1);
free(temp_s2);*/
/*
temp_s1 = v3_str(ps[i].v);
temp_s2 = v3_str(ps[j].v);
printf("collision vel: %s %s\n", temp_s1, temp_s2);
free(temp_s1);
free(temp_s2);
*/
temp = v3_copy(ps[i].v);
v3_sub(temp, ps[j].v);
/*
temp_s1 = v3_str(temp);
printf("temp = %s\n", temp_s1);
free(temp_s1);
*/
disp = v3_copy(ps[i].c);
v3_sub(disp, ps[j].c);
// disp2 = v3_copy(disp);
dm = RADIUS - v3_mag(disp) / 2;
/*
temp_s1 = v3_str(disp);
printf("disp = %s\n", temp_s1);
free(temp_s1);
*/
v3_unit(disp);
/*
temp_s1 = v3_str(disp);
printf("disp unit = %s\n", temp_s1);
free(temp_s1);
*/
dot = v3_dot(temp, disp);
/*
printf("dot = %f\n", dot);
*/
v3_mult_add(ps[i].v, disp, dm*LOSS_COL*dt);
v3_mult_sub(ps[j].v, disp, dm*LOSS_COL*dt);
// v3_mult_add(ps[i].c, disp, dm);
// v3_mult_sub(ps[j].c, disp, dm);
v3_del(disp);
v3_del(temp);
}
}
}
