static void raycast_light(t_hit *hit, unsigned depth)
{
t_lstiter it;
t_ray ray;
t_light *light;
int raycast_result;
t_vec3 lightness;
t_hit sub_hit;
if (depth == 0)
return ;
vec3_set(&lightness, 0, 0, 0);
init_iter(&it, rt.scene->lights, increasing);
while (lst_iterator_next(&it))
{
light = (t_light*)it.data;
vec3_copy(&ray.origin, &hit->position);
vec3_copy(&ray.direction, &light->position);
vec3_sub(&ray.direction, &hit->position);
vec3_normalize(&ray.direction);
ray.origin.x += ray.direction.x * RC_SHADOW_SHIFT;
ray.origin.y += ray.direction.y * RC_SHADOW_SHIFT;
ray.origin.z += ray.direction.z * RC_SHADOW_SHIFT;
raycast_result = raycast(&ray, &sub_hit, depth - 1, NULL);
if (sub_hit.object != NULL)
{
// ray bounce
}
else
{
vec3_add(&lightness, &light->color);
}
}
vec3_div(&lightness, rt.scene->lights->size);
vec3_add(&lightness, &rt.scene->ambient_light);
color_clamp(&lightness);
hit->color.x *= lightness.x;
hit->color.y *= lightness.y;
hit->color.z *= lightness.z;
}
static void terrain_new_chunk(terrain* ter, int i) {
const int SUBDIVISIONS = NUM_TERRAIN_SUBDIVISIONS+1;
terrain_chunk* tc = malloc(sizeof(terrain_chunk));
tc->id = i;
tc->x = i % ter->num_cols;
tc->y = i / ter->num_cols;
tc->width = ter->chunk_width;
tc->height = ter->chunk_height;
int x_max = tc->width * SUBDIVISIONS + 1;
int y_max = tc->height * SUBDIVISIONS + 1;
tc->num_verts = x_max * y_max + x_max * 2 + y_max * 2;
vec3* vertex_buffer = malloc(sizeof(vec3) * 4 * tc->num_verts);
int index = 0;
for(int x = 0; x < x_max; x++)
for(int y = 0; y < y_max; y++) {
float gx = tc->x * ter->chunk_width + (float)x/SUBDIVISIONS;
float gy = tc->y * ter->chunk_height + (float)y/SUBDIVISIONS;
float height = terrain_height(ter, vec2_new(gx, gy));
mat3 axis = terrain_tbn(ter, vec2_new(gx, gy));
vec3 pos = vec3_new(gx, height, gy);
vec3 tangent = mat3_mul_vec3(axis, vec3_new(1,0,0));
vec3 normal = mat3_mul_vec3(axis, vec3_new(0,1,0));
vec3 binorm = mat3_mul_vec3(axis, vec3_new(0,0,1));
vertex_buffer[index] = pos; index++;
vertex_buffer[index] = normal; index++;
vertex_buffer[index] = tangent; index++;
vertex_buffer[index] = binorm; index++;
}
/* Adding fins. Don't look, horrible code */
const float FIN_DEPTH = 5.0;
for(int y = 0; y < y_max; y++) {
int gx = tc->x * ter->chunk_width + 0;
int gy = tc->y * ter->chunk_height + (float)y/SUBDIVISIONS;
float height = terrain_height(ter, vec2_new(gx, gy)) - FIN_DEPTH;
mat3 axis = terrain_tbn(ter, vec2_new(gx, gy));
vec3 pos = vec3_new(gx, height, gy);
vec3 tangent = mat3_mul_vec3(axis, vec3_new(1,0,0));
vec3 normal = mat3_mul_vec3(axis, vec3_new(0,1,0));
vec3 binorm = mat3_mul_vec3(axis, vec3_new(0,0,1));
vertex_buffer[index] = pos; index++;
vertex_buffer[index] = normal; index++;
vertex_buffer[index] = tangent; index++;
vertex_buffer[index] = binorm; index++;
}
for(int y = 0; y < y_max; y++) {
int gx = tc->x * ter->chunk_width + ter->chunk_width;
int gy = tc->y * ter->chunk_height + (float)y/SUBDIVISIONS;
float height = terrain_height(ter, vec2_new(gx, gy)) - FIN_DEPTH;
mat3 axis = terrain_tbn(ter, vec2_new(gx, gy));
vec3 pos = vec3_new(gx, height, gy);
vec3 tangent = mat3_mul_vec3(axis, vec3_new(1,0,0));
vec3 normal = mat3_mul_vec3(axis, vec3_new(0,1,0));
vec3 binorm = mat3_mul_vec3(axis, vec3_new(0,0,1));
vertex_buffer[index] = pos; index++;
vertex_buffer[index] = normal; index++;
vertex_buffer[index] = tangent; index++;
vertex_buffer[index] = binorm; index++;
}
for(int x = 0; x < x_max; x++) {
int gx = tc->x * ter->chunk_width + (float)x/SUBDIVISIONS;
int gy = tc->y * ter->chunk_height + 0;
float height = terrain_height(ter, vec2_new(gx, gy)) - FIN_DEPTH;
mat3 axis = terrain_tbn(ter, vec2_new(gx, gy));
vec3 pos = vec3_new(gx, height, gy);
vec3 tangent = mat3_mul_vec3(axis, vec3_new(1,0,0));
vec3 normal = mat3_mul_vec3(axis, vec3_new(0,1,0));
vec3 binorm = mat3_mul_vec3(axis, vec3_new(0,0,1));
vertex_buffer[index] = pos; index++;
vertex_buffer[index] = normal; index++;
vertex_buffer[index] = tangent; index++;
vertex_buffer[index] = binorm; index++;
}
for(int x = 0; x < x_max; x++) {
int gx = tc->x * ter->chunk_width + (float)x/SUBDIVISIONS;
int gy = tc->y * ter->chunk_height + ter->chunk_height;
float height = terrain_height(ter, vec2_new(gx, gy)) - FIN_DEPTH;
mat3 axis = terrain_tbn(ter, vec2_new(gx, gy));
vec3 pos = vec3_new(gx, height, gy);
vec3 tangent = mat3_mul_vec3(axis, vec3_new(1,0,0));
vec3 normal = mat3_mul_vec3(axis, vec3_new(0,1,0));
vec3 binorm = mat3_mul_vec3(axis, vec3_new(0,0,1));
vertex_buffer[index] = pos; index++;
vertex_buffer[index] = normal; index++;
vertex_buffer[index] = tangent; index++;
vertex_buffer[index] = binorm; index++;
}
tc->bound.center = vec3_zero();
for (int i = 0; i < index; i+=4) {
tc->bound.center = vec3_add(tc->bound.center, vertex_buffer[i]);
}
tc->bound.center = vec3_div(tc->bound.center, tc->num_verts);
tc->bound.radius = 0;
for (int i = 0; i < index; i+=4) {
tc->bound.radius = max(tc->bound.radius, vec3_dist(tc->bound.center, vertex_buffer[i]));
}
if (net_is_client()) {
glGenBuffers(1, &tc->vertex_buffer);
glBindBuffer(GL_ARRAY_BUFFER, tc->vertex_buffer);
glBufferData(GL_ARRAY_BUFFER, sizeof(vec3) * 4 * tc->num_verts, vertex_buffer, GL_STATIC_DRAW);
}
free(vertex_buffer);
if (net_is_client()) {
glGenBuffers(NUM_TERRAIN_BUFFERS, tc->index_buffers);
}
for(int j = 0; j < NUM_TERRAIN_BUFFERS; j++) {
int off = pow(2, j);
int x_max = tc->width * SUBDIVISIONS;
int y_max = tc->height * SUBDIVISIONS;
tc->num_indicies[j] = (x_max / off) * (y_max / off) * 6 + (x_max / off) * 12 + (y_max / off) * 12;
uint32_t* index_buffer = malloc(sizeof(uint32_t) * tc->num_indicies[j]);
index = 0;
for(int x = 0; x < x_max; x+=off)
for(int y = 0; y < y_max; y+=off) {
index_buffer[index] = x + y * (x_max+1); index++;
index_buffer[index] = (x+off) + y * (x_max+1); index++;
index_buffer[index] = (x+off) + (y+off) * (x_max+1); index++;
index_buffer[index] = x + y * (x_max+1); index++;
index_buffer[index] = (x+off) + (y+off) * (x_max+1); index++;
index_buffer[index] = x + (y+off) * (x_max+1); index++;
}
/* Again, adding fins. Don't look horrible code */
int x_base = (x_max + 1) * (y_max + 1);
int y_base = (x_max + 1) * (y_max + 1) + (x_max + 1) * 2;
for(int x = 0; x < x_max; x+=off) {
index_buffer[index] = x + 0 * (x_max+1); index++;
index_buffer[index] = x_base + x; index++;
index_buffer[index] = (x+off) + 0 * (x_max+1); index++;
index_buffer[index] = (x+off) + 0 * (x_max+1); index++;
index_buffer[index] = x_base + x; index++;
index_buffer[index] = x_base + x+off; index++;
}
for(int x = 0; x < x_max; x+=off) {
index_buffer[index] = x + y_max * (x_max+1); index++;
index_buffer[index] = (x+off) + y_max * (x_max+1); index++;
index_buffer[index] = x_base + y_max+1 + x; index++;
index_buffer[index] = (x+off) + y_max * (x_max+1); index++;
index_buffer[index] = x_base + x_max+1 + x+off; index++;
index_buffer[index] = x_base + x_max+1 + x; index++;
}
for(int y = 0; y < y_max; y+=off) {
index_buffer[index] = 0 + y * (x_max+1); index++;
index_buffer[index] = 0 + (y+off) * (x_max+1); index++;
index_buffer[index] = y_base + y; index++;
index_buffer[index] = 0 + (y+off) * (x_max+1); index++;
index_buffer[index] = y_base + y+off; index++;
index_buffer[index] = y_base + y; index++;
}
for(int y = 0; y < y_max; y+=off) {
index_buffer[index] = x_max + y * (x_max+1); index++;
index_buffer[index] = y_base + y_max+1 + y; index++;
index_buffer[index] = x_max + (y+off) * (x_max+1); index++;
index_buffer[index] = x_max + (y+off) * (x_max+1); index++;
index_buffer[index] = y_base + y_max+1 + y; index++;
index_buffer[index] = y_base + y_max+1 + y+off; index++;
}
if (net_is_client()) {
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, tc->index_buffers[j]);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * tc->num_indicies[j], index_buffer, GL_DYNAMIC_DRAW);
}
free(index_buffer);
}
tc->colmesh = malloc(sizeof(cmesh));
tc->colmesh->is_leaf = true;
tc->colmesh->triangles_num = (tc->width/4) * (tc->height/4) * 2;
tc->colmesh->triangles = malloc(sizeof(ctri) * tc->colmesh->triangles_num);
int tri_i = 0;
for (int x = 0; x < tc->width; x += 4)
for (int y = 0; y < tc->height; y += 4) {
float gx = tc->x * ter->chunk_width + (float)x;
float gy = tc->y * ter->chunk_height + (float)y;
vec3 a = vec3_new(gx , terrain_height(ter, vec2_new(gx , gy )) , gy );
vec3 b = vec3_new(gx+4, terrain_height(ter, vec2_new(gx+4, gy )) , gy );
vec3 c = vec3_new(gx+4, terrain_height(ter, vec2_new(gx+4, gy+4)) , gy+4);
vec3 d = vec3_new(gx , terrain_height(ter, vec2_new(gx , gy+4)) , gy+4);
vec3 tang = vec3_normalize(vec3_sub(b, a));
vec3 binorm = vec3_normalize(vec3_sub(d, a));
vec3 norm = vec3_cross( binorm, tang );
tc->colmesh->triangles[tri_i] = ctri_new(a, c, b, norm); tri_i++;
tc->colmesh->triangles[tri_i] = ctri_new(a, d, c, norm); tri_i++;
}
tc->colmesh->bound = cmesh_bound(tc->colmesh);
/* For some reason this is not working correctly */
cmesh_subdivide(tc->colmesh, 5);
ter->chunks[i] = tc;
}
union vec3 *vec3_div_self(union vec3 *vi, float scalar)
{
return vec3_div(vi, vi, scalar);
}
vec3_t vec3_normalize(const vec3_t v) {
if (vec3_equal(v, vec3zero))
return vec3zero;
else
return vec3_div(v, vec3_len(v));
}
void objpose(mat33_t *R, vec3_t *t, int *it, real_t *obj_err, real_t *img_err,
bool calc_img_err, const vec3_t *_P, const vec3_t *Qp, const options_t options, const int n)
{
int i, j;
//vec3_array P(_P.begin(),_P.end());
vec3_t P[n];
memcpy(&*P, _P, n*sizeof(vec3_t));
//const int n = (unsigned int) P.size();
vec3_t pbar;
vec3_array_sum(&pbar, &*P, n);
vec3_div(&pbar, (real_t)(n));
vec3_array_sub(&*P, &pbar, n);
//vec3_array Q(Qp.begin(),Qp.end());
vec3_t Q[n];
memcpy(&*Q, Qp, n*sizeof(vec3_t));
vec3_t ones;
ones.v[0] = 1;
ones.v[1] = 1;
ones.v[2] = 1;
const bool mask_z[3] = {0,0,1};
vec3_array_set(&*Q, &ones, mask_z, n);
//mat33_array F;
//F.resize(n);
mat33_t F[n];
vec3_t V;
for(i=0; i<n; i++)
{
V.v[0] = Q[i].v[0] / Q[i].v[2];
V.v[1] = Q[i].v[1] / Q[i].v[2];
V.v[2] = 1.0;
mat33_t _m;
vec3_mul_vec3trans(&_m, &V, &V);
mat33_div(&_m, vec3trans_mul_vec3(&V,&V));
F[i] = _m;
}
mat33_t tFactor;
mat33_t _m1,_m2,_m3;
mat33_eye(&_m1);
mat33_array_sum(&_m2, &*F, n);
mat33_div(&_m2, (real_t)(n));
mat33_sub_mat2(&_m3, &_m1, &_m2);
mat33_inv(&tFactor, &_m3);
mat33_div(&tFactor, (real_t)(n));
*it = 0;
int initR_approximate = mat33_all_zeros(&options.initR);
mat33_t Ri;
vec3_t ti;
//vec3_array Qi;
//Qi.resize(n);
vec3_t Qi[n];
real_t old_err = 0.0, new_err = 0.0;
// ----------------------------------------------------------------------------------------
if(initR_approximate == 0)
{
mat33_copy(&Ri, &options.initR);
vec3_t _sum;
vec3_t _v1, _v2;
mat33_t _m1,_m2;
vec3_clear(&_sum);
for(j=0; j<n; j++)
{
mat33_eye(&_m1);
mat33_sub_mat2(&_m2, &F[j], &_m1);
vec3_mult_mat(&_v1, &Ri, &P[j]);
vec3_mult_mat(&_v2, &_m2, &_v1);
vec3_add_vec(&_sum, &_v2);
}
vec3_mult_mat(&ti,&tFactor,&_sum);
xform(&*Qi, &*P, &Ri, &ti, n);
old_err = 0;
vec3_t _v;
for(j=0; j<n; j++)
{
mat33_eye(&_m1);
mat33_sub_mat2(&_m2, &F[j], &_m1);
vec3_mult_mat(&_v, &_m2, &Qi[j]);
old_err += vec3_dot(&_v, &_v);
}
// ----------------------------------------------------------------------------------------
}
else
{
abskernel(&Ri, &ti, &*Qi, &old_err, &*P, &*Q, &*F, &tFactor, n);
*it = 1;
}
// ----------------------------------------------------------------------------------------
abskernel(&Ri, &ti, &*Qi, &new_err, &*P, &*Qi, &*F, &tFactor, n);
*it = *it + 1;
while((_abs((old_err-new_err)/old_err) > options.tol) && (new_err > options.epsilon) &&
(options.max_iter == 0 || *it < options.max_iter))
{
old_err = new_err;
abskernel(&Ri, &ti, &*Qi, &new_err, &*P, &*Qi, &*F, &tFactor, n);
*it = *it + 1;
}
mat33_copy(R, &Ri);
vec3_copy(t, &ti);
*obj_err = _sqrt(new_err/(real_t)(n));
if(calc_img_err == 1)
{
//vec3_array Qproj;
//Qproj.resize(n);
vec3_t Qproj[n];
xformproj(&*Qproj, &*P, &Ri, &ti, n);
*img_err = 0;
vec3_t _v;
for(j=0; j<n; j++)
{
vec3_sub_vec2(&_v, &Qproj[j], &Qp[j]);
*img_err += vec3_dot(&_v, &_v);
}
*img_err = _sqrt(*img_err/(real_t)(n));
}
if(t->v[2] < 0)
{
mat33_mult(R, -1.0);
vec3_mult(t, -1.0);
}
vec3_t _ts;
vec3_mult_mat(&_ts, &Ri, &pbar);
vec3_sub_vec(t, &_ts);
}
// ===========================================================================================
void abskernel(mat33_t *R, vec3_t *t, vec3_t *Qout, real_t *err2,
const vec3_t *_P, const vec3_t *_Q,
const mat33_t *F, const mat33_t *G, const int n)
{
int i, j;
vec3_t P[n];//(_P.begin(),_P.end());
vec3_t Q[n];//(_Q.begin(),_Q.end());
//const unsigned int n = (unsigned int) P.size();
memcpy(&*P, _P, n*sizeof(vec3_t));
memcpy(&*Q, _Q, n*sizeof(vec3_t));
for(i=0; i<n; i++)
{
vec3_t _q;
vec3_mult_mat(&_q, &F[i], &_Q[i]);
vec3_copy(&Q[i], &_q);
}
vec3_t pbar;
vec3_array_sum(&pbar, &*P, n);
vec3_div(&pbar, (real_t)(n));
vec3_array_sub(&*P, &pbar, n);
vec3_t qbar;
vec3_array_sum(&qbar, &*Q, n);
vec3_div(&qbar, (real_t)(n));
vec3_array_sub(&*Q, &qbar, n);
mat33_t M;
mat33_clear(&M);
for(j=0; j<n; j++)
{
mat33_t _m;
vec3_mul_vec3trans(&_m, &P[j], &Q[j]);
mat33_add(&M, &_m);
}
mat33_t _U;
mat33_t _S;
mat33_t _V;
mat33_clear(&_U);
mat33_clear(&_S);
mat33_clear(&_V);
mat33_svd2(&_U, &_S, &_V, &M);
mat33_t _Ut;
mat33_transpose(&_Ut, _U);
mat33_mult_mat2(R, &_V, &_Ut);
// R = V*(U.');
// WE need to check the determinant of the R
real_t det = mat33_det(R);
if ( det < 0 )
{
mat33_t Vc;
Vc.m[0] = _V.m[0]; Vc.m[1] = _V.m[1]; Vc.m[2] = -_V.m[2];
Vc.m[3] = _V.m[3]; Vc.m[4] = _V.m[4]; Vc.m[5] = -_V.m[5];
Vc.m[6] = _V.m[6]; Vc.m[7] = _V.m[7]; Vc.m[8] = -_V.m[8];
mat33_mult_mat2(R, &Vc, &_Ut); // to have det(R) == 1
optimal_t(t, R, G, F, &*P, n);
if( t->v[2] < 0 ){
Vc.m[0] =- _V.m[0]; Vc.m[1] =- _V.m[1]; Vc.m[2] = -_V.m[2];
Vc.m[3] =- _V.m[3]; Vc.m[4] =- _V.m[4]; Vc.m[5] = -_V.m[5];
Vc.m[6] =- _V.m[6]; Vc.m[7] =- _V.m[7]; Vc.m[8] = -_V.m[8];
mat33_mult_mat2(R, &Vc, &_Ut); // to have det(R) == 1 & t_3 > 0
optimal_t(t, R, G, F, &*P, n);
}
}else{
optimal_t(t,R,G,F,&*P,n);
if( t->v[2] < 0 )
{
mat33_t Vc;
Vc.m[0] = -_V.m[0]; Vc.m[1] = -_V.m[1]; Vc.m[2] = _V.m[2];
Vc.m[3] = -_V.m[3]; Vc.m[4] = -_V.m[4]; Vc.m[5] = _V.m[5];
Vc.m[6] = -_V.m[6]; Vc.m[7] = -_V.m[7]; Vc.m[8] = _V.m[8];
mat33_mult_mat2(R, &Vc, &_Ut);
optimal_t(t,R,G,F,&*P,n);
}
}
// CHECK if everything is working ok
// check the det of R
//det = mat33_det(R);
xform(Qout,&*P,R,t, n);
*err2 = 0.0;
mat33_t _m1;
vec3_t _v1;
for(i=0; i<n; i++)
{
mat33_eye(&_m1);
mat33_sub(&_m1, &F[i]);
vec3_mult_mat(&_v1, &_m1, &Qout[i]);
*err2 += vec3_dot(&_v1, &_v1);
}
}
