示例#1
0
文件: materials.c 项目: Raphy42/RT
t_bool          dielectric(t_material *material, const t_ray *r, const t_hit_record *h, t_vec3 *attenuation, t_ray *scattered)
{
    t_vec3      outward_normal;
    t_vec3      reflected;
    t_vec3      refracted;
    float       ni_over_nt;
    float       reflect_probe;
    float       cosine;

    vec3_reflect(&reflected, &RAY_DIRECTION(r), &h->normal);
    vec3_assign(attenuation, &material->texture.albedo);
    if (vec3_dot(&RAY_DIRECTION(r), &h->normal) > 0)
    {
        vec3_mul_f(&outward_normal, &h->normal, -1.f);
        //TODO pass idx
        ni_over_nt = REF_IDX;
        cosine = REF_IDX * vec3_dot(&RAY_DIRECTION(r), &h->normal) / vec3_length(&RAY_DIRECTION(r));
    }
    else
    {
        vec3_assign(&outward_normal, &h->normal);
        ni_over_nt = 1.0f / REF_IDX;
        cosine = - vec3_dot(&RAY_DIRECTION(r), &h->normal) / vec3_length(&RAY_DIRECTION(r));
    }
    if (refract(&RAY_DIRECTION(r), &outward_normal, ni_over_nt, &refracted))
        reflect_probe = schlick(cosine, REF_IDX);
    else
        reflect_probe = 1.0f;
    if (drand48() < reflect_probe)
        ray_assign(scattered, &h->pos, &reflected);
    else
        ray_assign(scattered, &h->pos, &refracted);
    return (TRUE);
}
示例#2
0
文件: entity.c 项目: Raphy42/RT
t_primitive_rectangle   *rectangle_create(const t_vec3 *p0, const t_vec3 *a, const t_vec3 *b)
{
    t_primitive_rectangle   *r;

    r = (t_primitive_rectangle *)ft_memalloc(sizeof(t_primitive_rectangle));
    vec3_assign(&r->p0, p0);
    vec3_assign(&r->a, a);
    vec3_assign(&r->b, b);
    return (r);
}
示例#3
0
文件: entity.c 项目: Raphy42/RT
t_primitive_triangle    *triangle_create(const t_vec3 *v0, const t_vec3 *v1, const t_vec3 *v2)
{
    t_primitive_triangle    *t;

    t = (t_primitive_triangle *)ft_memalloc(sizeof(t_primitive_triangle));
    vec3_assign(&t->v0, v0);
    vec3_assign(&t->v1, v1);
    vec3_assign(&t->v2, v2);
    return (t);
}
示例#4
0
// ===========================================================================================
void decomposeR(mat33_t *Rz, const mat33_t *R)
{
	real_t cl = _atan2(R->m[7],R->m[6]);
	vec3_t rpy;
	vec3_assign(&rpy,0,0,cl);
	rpyMat(Rz,&rpy);
}
示例#5
0
文件: entity.c 项目: Raphy42/RT
t_material      *material_create(t_material_type type, t_vec3 *albedo_t)
{
    t_material  *material;

    material = (t_material *)ft_memalloc(sizeof(t_material));
    material->type = type;
    vec3_assign(&material->texture.albedo, albedo_t);
    if (material->type == MATERIAL_LAMBERTIAN)
    {
        material->scatter = &lambertian;
        material->texture.value = &albedo;
    }
    else if (material->type == MATERIAL_METAL)
        material->scatter = &metal;
    else if (material->type == MATERIAL_DIELECTRIC)
        material->scatter = &dielectric;
    else if (material->type == MATERIAL_DEBUG)
    {
        material->scatter = &lambertian;
        material->texture.value = &checker;
    }
    else if (material->type == MATERIAL_EMITTER)
    {
        material->scatter = &emitter;
        material->emit = &basic_light;
    }
    return (material);
}
示例#6
0
文件: entity.c 项目: Raphy42/RT
t_primitive_box     *box_create(const t_vec3 *a, const t_vec3 *b)
{
    t_primitive_box *box;

    box = (t_primitive_box *)ft_memalloc(sizeof(t_primitive_box));
    if (a->x < b->x && a->y < b->y && a->z < b->z)
    {
        vec3_assign(&box->a, a);
        vec3_assign(&box->b, b);
    }
    else
    {
        vec3_assign(&box->a, b);
        vec3_assign(&box->b, a);
    }
    return (box);
}
示例#7
0
Box* initBox(const mat4 inv_transform, const float x_span, const float y_span, const float z_span, Material* mat)
{
    float half_x = x_span / 2.0f;
    float half_y = y_span / 2.0f;
    float half_z = z_span / 2.0f;
    AABox* aabox = (AABox*)malloc(sizeof(AABox));
    vec3_assign(aabox->min, -half_x, -half_y, -half_z);
    vec3_assign(aabox->max, half_x, half_y, half_z);
    aabox->mat = mat;

    Box* box = (Box*)malloc(sizeof(Box));
    box->obj.ptr = aabox;
    box->obj.type = AABOX;
    box->mat = mat;
    mat4_copy(box->inv_transform, inv_transform);
    return box;
}
示例#8
0
// ===========================================================================================
void rpyAng(vec3_t *angs, const mat33_t *R)
{
	const real_t sinB = -(R->m[6]);
	const real_t cosB = _sqrt(R->m[0]*R->m[0] + R->m[3]*R->m[3]);

	if(_abs(cosB) > (real_t)(1E-15))
	{
		const real_t sinA = R->m[3] / cosB;
		const real_t cosA = R->m[0] / cosB;
		const real_t sinC = R->m[7] / cosB;
		const real_t cosC = R->m[8] / cosB;
		vec3_assign(angs,_atan2(sinC,cosC),_atan2(sinB,cosB),_atan2(sinA,cosA));
	}
	else
	{
		const real_t sinC = (R->m[1] - R->m[5]) * 0.5;
		const real_t cosC = (R->m[4] - R->m[2]) * 0.5;
		vec3_assign(angs,_atan2(sinC,cosC),CONST_PI_OVER_2, 0.0);
	}
}
示例#9
0
文件: materials.c 项目: Raphy42/RT
t_bool           metal(t_material *material, const t_ray *r, const t_hit_record *h, t_vec3 *attenuation, t_ray *scattered)
{
    t_vec3      reflected;
    t_vec3      random;

    random = random_cosine_direction(.3f);
    vec3_reflect(&reflected, vec3_unit_vector(&reflected, &RAY_DIRECTION(r)), &h->normal);
    ray_assign(scattered, &h->pos, &reflected);
    vec3_assign(attenuation, &material->texture.albedo);
    return (vec3_dot(&RAY_DIRECTION(scattered), &h->normal) > 0);
}
示例#10
0
void rpyAng_X(vec3_t *ang_zyx, const mat33_t *R)
{
	rpyAng(ang_zyx,R);

	if(_abs(ang_zyx->v[0]) > CONST_PI_OVER_2)
	{
		while(_abs(ang_zyx->v[0]) > CONST_PI_OVER_2)
		{
			if(ang_zyx->v[0] > 0)
			{
				vec3_assign(ang_zyx, ang_zyx->v[0]+CONST_PI,
					                 3*CONST_PI-ang_zyx->v[1],
									 ang_zyx->v[2]+CONST_PI);
				vec3_sub(ang_zyx,CONST_2_PI);
			}
			else
			{
				vec3_assign(ang_zyx, ang_zyx->v[0]+CONST_PI,
									 3*CONST_PI-ang_zyx->v[1],
									 ang_zyx->v[2]+CONST_PI);
			}
		}
	}
}
示例#11
0
文件: entity.c 项目: Raphy42/RT
t_entity        *entity_create(t_entity_type type, t_material *material, const t_vec3 *pos, void *data)
{
    t_entity    *entity;

    entity = (t_entity *)ft_memalloc(sizeof(t_entity));
    entity->type = type;
    if (type == PRIMITIVE_SPHERE)
    {
        vec3_assign(&entity->center, pos);
        entity->hit = &sphere_hit;
    }
    else if (type == PRIMITIVE_TRIANGLE)
        entity->hit = &triangle_hit;
    else if (type == PRIMITIVE_RECTANGLE)
        entity->hit = &rectangle_hit;
    else if (type == PRIMITIVE_AXIS_ALIGNED_BOX)
        entity->hit = &box_hit;
    entity->material = material;
    entity->data = data;
    return (entity);
}
示例#12
0
// Utility function for vol_draw_mandel_box ().
double _vol_draw_mandel_box_equation (double *v, double s, double r, double f, double *c)
{
  //printf ("TEST %lf %lf %lf: %lf %lf %lf, %lf %lf %lf\n", v[0], v[1], v[2], s, r, f, c[0], c[1], c[2]);
  vec3_clone (fold, v);
  int i;
  for (i = 0; i < 3; i++)
    {
      if (fold[i] > 1)       fold[i] = (double) 2.0 - fold[i];
      else if (fold[i] < -1) fold[i] = (double) -2.0 - fold[i];
    }

  vec3_s_mul (fold, f);
  double m = vec3_len (fold);
  if (m < r)      { vec3_s_mul (fold, 4); }
  else if (m < 1) { vec3_s_div (fold, m * m); }
  vec3_assign (v, fold);
  vec3_s_mul (v, s);
  vec3_add (v, c);

  return vec3_len (v);
}
示例#13
0
文件: text.c 项目: andrewrch/orrery
void render_text(FontRenderer* r, FontAtlas* a, const char *text,
                 vec4 coords, float sx, float sy) {
  bind_program(&r->sp);
  const uint8_t *p;
  /* Use the texture containing the atlas */
  bind_texture(&a->t);
  add_int_uniform(&r->sp, "tex", 0);

  glBindVertexArray(r->vao);
  BufferData b[6* strlen(text)];
  int c = 0;
  float x = coords[0], y = coords[1], z = coords[2];
  /* Loop through all characters */
  for (p = (const uint8_t *)text; *p; p++) {
    /* Calculate the vertex and texture coordinates */
    float x2 = x + a->c[*p].bl * sx;
    float y2 = -y - a->c[*p].bt * sy;
    float w = a->c[*p].bw * sx;
    float h = a->c[*p].bh * sy;

    /* Advance the cursor to the start of the next character */
    x += a->c[*p].ax * sx;
    y += a->c[*p].ay * sy;

    /* Skip glyphs that have no pixels */
    if (!w || !h)
      continue;

    float tx0, tx1, ty0, ty1, px0, px1, py0, py1;
    // Clockwise winding with positions named like this
    //  x0,y1 o---o x1,y1
    //        |  /|
    //        | / |
    //        |/  |
    //  x0,y0 o---o x1,y0

    px0 = x2;
    px1 = x2 + w;
    py0 = -y2 - h;
    py1 = -y2;

    tx0 = a->c[*p].tx;
    tx1 = a->c[*p].tx + a->c[*p].bw / a->w,
    ty0 = a->c[*p].ty + a->c[*p].bh / a->h;
    ty1 = a->c[*p].ty;

    vec3_assign(b[c].pos,   px0, py0, z);
    vec2_assign(b[c++].tex, tx0, ty0);
    vec3_assign(b[c].pos,   px1, py1, z);
    vec2_assign(b[c++].tex, tx1, ty1);
    vec3_assign(b[c].pos,   px0, py1, z);
    vec2_assign(b[c++].tex, tx0, ty1);
    // Second triangle
    vec3_assign(b[c].pos,   px1, py1, z);
    vec2_assign(b[c++].tex, tx1, ty1);
    vec3_assign(b[c].pos,   px0, py0, z);
    vec2_assign(b[c++].tex, tx0, ty0);
    vec3_assign(b[c].pos,   px1, py0, z);
    vec2_assign(b[c++].tex, tx1, ty0);
  }
  /* Draw all the character on the screen in one go */
  glBindBuffer(GL_ARRAY_BUFFER, r->vbo);
  glBufferData(GL_ARRAY_BUFFER, sizeof(b), b, GL_DYNAMIC_DRAW);
  glDrawArrays(GL_TRIANGLES, 0, c);
  glBindVertexArray(0);
}
示例#14
0
文件: ray.c 项目: Raphy42/RT
t_ray       *ray_assign(t_ray *r, const t_vec3 *a, const t_vec3 *b)
{
    vec3_assign(&r->a, a);
    vec3_assign(&r->b, b);
    return (r);
}
示例#15
0
void getRfor2ndPose_V_Exact(pose_t *sol, const vec3_t *v, const vec3_t *P,
					        const mat33_t R, const vec3_t t, const real_t DB, const int n, int *r_n)
{

	mat33_t RzN;
	decomposeR(&RzN, &R);
	mat33_t R_;
	mat33_mult_mat2(&R_, &R, &RzN);
	mat33_t RzN_tr;
	mat33_transpose(&RzN_tr, RzN);
	//vec3_array P_;
	vec3_t P_[n];
	
	vec3_array_mult_vec2(&*P_, &RzN_tr, P, n);
	
	vec3_t ang_zyx;
	rpyAng_X(&ang_zyx, &R_);
	
	vec3_t rpy;
	mat33_t Ry,Rz;
	vec3_assign(&rpy, 0, ang_zyx.v[1], 0);
	rpyMat(&Ry, &rpy);
	vec3_assign(&rpy,0,0,ang_zyx.v[2]);
	rpyMat(&Rz,&rpy);
	//scalar_array bl;
	//vec3_array Tnew;
	//double *bl = (double*)malloc(4 * sizeof(double));
	//vec3_t *Tnew = (vec3_t*)malloc(4 * sizeof(vec3_t));
	double bl[10];
	vec3_t Tnew[10];
	
	int res_n = 0;
	
	getRotationY_wrtT(&*bl, &*Tnew, v, &*P_, &t, &DB, &Rz, n, &res_n);
	
	// Estimate the Error for all solutions !
	scalar_array_div(bl, 180.0/CONST_PI, res_n);
	
	mat33_t V[n];
	
	int i, j;
	for(i=0; i<n; i++)
	{
		vec3_mul_vec3trans(&V[i], &v[i], &v[i]);
		mat33_div(&V[i], vec3trans_mul_vec3(&v[i],&v[i]));
	}
	
	mat33_t _m1;
	mat33_t _m2;
	vec3_t _v1;
	vec3_t _v2;

	for(j=0; j<res_n; j++)
	{
		mat33_clear(&Ry);
		vec3_assign(&rpy, 0, bl[j], 0);
		rpyMat(&Ry, &rpy);
		mat33_mult_mat2(&_m1, &Rz, &Ry);
		mat33_mult_mat2(&sol[j].R, &_m1, &RzN_tr);
		vec3_copy(&sol[j].t, &Tnew[j]);
		real_t E = 0;
		for(i=0; i<n; i++)
		{
			mat33_eye(&_m2);
			mat33_sub(&_m2, &V[i]);
			vec3_mult_mat(&_v1, &sol[j].R, &P[i]);
			vec3_add_vec(&_v1, &sol[j].t);
			vec3_mult_mat(&_v2, &_m2, &_v1);
			vec3_mult_vec(&_v2, &_v2);
			E += vec3_sum(&_v2);
		}
		sol[j].E = E;
	}
	
	*r_n = res_n;
}
示例#16
0
void getRotationY_wrtT(double *al_ret, vec3_t *tnew, const vec3_t *v,
					   const vec3_t *p, const vec3_t *t, const real_t *DB,
					   const mat33_t *Rz, const int n, int *res_n)
{
	int i,j,k;
	
	mat33_t V[n];
	
	for(i=0; i<n; i++)
	{
		vec3_mul_vec3trans(&V[i], &v[i], &v[i]);
		mat33_div(&V[i], vec3trans_mul_vec3(&v[i], &v[i]));
	}

	mat33_t G, _g1, _g2, _g3;
	mat33_array_sum(&_g1, &*V, n);
	mat33_eye(&_g2);
	mat33_div(&_g1, (real_t)(n));
	mat33_sub_mat2(&_g3, &_g2, &_g1);
	mat33_inv(&G, &_g3);
	mat33_div(&G, (real_t)(n));
	mat33_t _opt_t;
	mat33_clear(&_opt_t);

	for(i=0; i<n; i++)
	{
		const real_t v11 = V[i].m[0]; 
		const real_t v21 = V[i].m[3];
		const real_t v31 = V[i].m[6];
		const real_t v12 = V[i].m[1]; 
		const real_t v22 = V[i].m[4];
		const real_t v32 = V[i].m[7];
		const real_t v13 = V[i].m[2]; 
		const real_t v23 = V[i].m[5];
		const real_t v33 = V[i].m[8];
		const real_t px = p[i].v[0];
		const real_t py = p[i].v[1];
		const real_t pz = p[i].v[2];
		const real_t r1 = Rz->m[0];
		const real_t r2 = Rz->m[1];
		const real_t r3 = Rz->m[2];
		const real_t r4 = Rz->m[3];
		const real_t r5 = Rz->m[4];
		const real_t r6 = Rz->m[5];
		const real_t r7 = Rz->m[6];
		const real_t r8 = Rz->m[7];
		const real_t r9 = Rz->m[8];

		mat33_t _o;
		_o.m[0] = (((v11-(real_t)(1))*r2+v12*r5+v13*r8)*py+(-(v11-(real_t)(1))*r1-v12*r4-v13*r7)*px+(-(v11-(real_t)(1))*r3-v12*r6-v13*r9)*pz);
		_o.m[1] = (((real_t)(2)*(v11-(real_t)(1))*r1+(real_t)(2)*v12*r4+(real_t)(2)*v13*r7)*pz+(-(real_t)(2)*(v11-(real_t)(1))*r3-(real_t)(2)*v12*r6-(real_t)(2)*v13*r9)*px);
		_o.m[2] = ((v11-(real_t)(1))*r1+v12*r4+v13*r7)*px+((v11-(real_t)(1))*r3+v12*r6+v13*r9)*pz+((v11-(real_t)(1))*r2+v12*r5+v13*r8)*py;

		_o.m[3] = ((v21*r2+(v22-(real_t)(1))*r5+v23*r8)*py+(-v21*r1-(v22-(real_t)(1))*r4-v23*r7)*px+(-v21*r3-(v22-(real_t)(1))*r6-v23*r9)*pz);
		_o.m[4] = (((real_t)(2)*v21*r1+(real_t)(2)*(v22-(real_t)(1))*r4+(real_t)(2)*v23*r7)*pz+(-(real_t)(2)*v21*r3-(real_t)(2)*(v22-(real_t)(1))*r6-(real_t)(2)*v23*r9)*px);
		_o.m[5] = (v21*r1+(v22-(real_t)(1))*r4+v23*r7)*px+(v21*r3+(v22-(real_t)(1))*r6+v23*r9)*pz+(v21*r2+(v22-(real_t)(1))*r5+v23*r8)*py;

		_o.m[6] = ((v31*r2+v32*r5+(v33-(real_t)(1))*r8)*py+(-v31*r1-v32*r4-(v33-(real_t)(1))*r7)*px+(-v31*r3-v32*r6-(v33-(real_t)(1))*r9)*pz);
		_o.m[7] = (((real_t)(2)*v31*r1+(real_t)(2)*v32*r4+(real_t)(2)*(v33-(real_t)(1))*r7)*pz+(-(real_t)(2)*v31*r3-(real_t)(2)*v32*r6-(real_t)(2)*(v33-(real_t)(1))*r9)*px);
		_o.m[8] = (v31*r1+v32*r4+(v33-(real_t)(1))*r7)*px+(v31*r3+v32*r6+(v33-(real_t)(1))*r9)*pz+(v31*r2+v32*r5+(v33-(real_t)(1))*r8)*py;

		mat33_add(&_opt_t, &_o);
	}

	mat33_t opt_t;
	mat33_mult_mat2(&opt_t, &G, &_opt_t);
	real_t E_2[5] = {0,0,0,0,0};
	for(i=0; i<n; i++)
	{
		const real_t px = p[i].v[0];
		const real_t py = p[i].v[1];
		const real_t pz = p[i].v[2];

		mat33_t Rpi;
		mat33_assign(&Rpi, -px, (real_t)(2)*pz,px,py,(real_t)(0),py,-pz,-(real_t)(2)*px,pz);

		mat33_t E,_e1,_e2;
		mat33_eye(&_e1);
		mat33_sub(&_e1, &V[i]);
		mat33_mult_mat2(&_e2, Rz, &Rpi);
		mat33_add(&_e2, &opt_t);
		mat33_mult_mat2(&E,&_e1,&_e2);
		vec3_t e2,e1,e0;
		mat33_to_col_vec3(&e2,&e1,&e0,&E);
		vec3_t _E2_0,_E2_1,_E2_2,_E2_3,_E2_4;
		vec3_copy(&_E2_0,&e2);
		vec3_mult_vec(&_E2_0,&e2);
		vec3_copy(&_E2_1,&e1);
		vec3_mult_vec(&_E2_1,&e2);
		vec3_mult(&_E2_1,2.0);
		vec3_copy(&_E2_2,&e0);
		vec3_mult_vec(&_E2_2,&e2);
		vec3_mult(&_E2_2,2.0);
		vec3_t _e1_sq;
		vec3_copy(&_e1_sq,&e1);
		vec3_mult_vec(&_e1_sq,&e1);
		vec3_add_vec(&_E2_2,&_e1_sq);
		vec3_copy(&_E2_3,&e0);
		vec3_mult_vec(&_E2_3,&e1);
		vec3_mult(&_E2_3,2.0);
		vec3_copy(&_E2_4,&e0);
		vec3_mult_vec(&_E2_4,&e0);
		E_2[0] += vec3_sum(&_E2_0);
		E_2[1] += vec3_sum(&_E2_1);
		E_2[2] += vec3_sum(&_E2_2);
		E_2[3] += vec3_sum(&_E2_3);
		E_2[4] += vec3_sum(&_E2_4);
	}

	//scalar_array _a;
	//_a.resize(5);
	double _a[5];
	
	_a[4] = -E_2[1];
	_a[3] = (real_t)(4)*E_2[0] - (real_t)(2)*E_2[2];
	_a[2] = -(real_t)(3)*E_2[3] + (real_t)(3)*E_2[1];
	_a[1] = -(real_t)(4)*E_2[4] + (real_t)(2)*E_2[2];
	_a[0] = E_2[3];

	double at_sol[5];
	
	int num_sol = solve_polynomial(&*at_sol, &*_a, 5);
	double e[num_sol];
	scalar_array_clear(&*e, num_sol);
	
	double at[num_sol];
	
	if(COMPLICATED_ERROR_CMP)
	{
	  // get the error in a complicate way
	  scalar_array_clear(&*e, num_sol);
	  scalar_array_add(&*e, _a[0], num_sol);
	  
	  //at.clear();
	  //at.assign(at_sol.begin(),at_sol.end());
	  memcpy(&*at, &*at_sol, num_sol*sizeof(double));
	  
	  scalar_array_mult(&*at, _a[1], num_sol);
	  scalar_array_add_vec(&*e, &*at, num_sol);
	  
	  for(j=2; j<=4; j++)
	    {
	      //at.clear();
	      //at.assign(at_sol.begin(),at_sol.end());
		  memcpy(&*at, &*at_sol, num_sol*sizeof(double));
		  
	      scalar_array_pow(&*at, (real_t)(j), num_sol);
	      scalar_array_mult(&*at, _a[j], num_sol);
	      scalar_array_add_vec(&*e, &*at, num_sol);
	    }
	}
	else
	{
	  // Or in a fast one 
	  scalar_array_add(&*e, _a[4], num_sol);
	  for(j=3;j>0;j--)
	  {
	    // multiply with at & add a_ 
	    for(k=0;k<num_sol;k++)
		{
	      e[k] =  e[k]*at_sol[k] + _a[j] ;
		}
	  }
	}
	
	memcpy(&*at, &*at_sol, num_sol*sizeof(double));

	// get the angle al
	//scalar_array sa(at.begin(),at.end());
	double sa[num_sol];
	memcpy(&*sa, &*at, num_sol*sizeof(double));
	
	scalar_array_mult(&*sa, 2.0, num_sol);
	
	//scalar_array _ca1(at.begin(),at.end());
	double _ca1[num_sol];
	memcpy(&*_ca1, &*at, num_sol*sizeof(double));
	
	scalar_array_pow(&*_ca1,2.0, num_sol);
	scalar_array_add(&*_ca1,1.0, num_sol);
	
	//scalar_array ca(at.begin(),at.end());
	double ca[num_sol];
	memcpy(&*ca, &*at, num_sol*sizeof(double));
	
	scalar_array_pow(&*ca,2, num_sol);
	scalar_array_negate(&*ca, num_sol);
	scalar_array_add(&*ca,1.0, num_sol);
	scalar_array_div_vec(&*ca, &*_ca1, num_sol);
	scalar_array_div_vec(&*sa, &*_ca1, num_sol);
	
	double al[num_sol];
	scalar_array_atan2(&*al, &*sa, &*ca, num_sol);
	
	// check the sign of the derivative 
	scalar_array_mult(&*al, (real_t)(180./CONST_PI), num_sol);
	
	double _c_tMaxMin[num_sol];
	//_c_tMaxMin.resize(at.size());
	scalar_array_clear(&*_c_tMaxMin, num_sol);
	scalar_array_add(&*_c_tMaxMin, _a[1], num_sol);
	double _at[num_sol];
	//_at.clear();
	//_at.assign(at.begin(),at.end());
	memcpy(&*_at, &*at, num_sol*sizeof(double));
	
	scalar_array_mult(&*_at, _a[2], num_sol);
	scalar_array_mult(&*_at, 2.0, num_sol);
	scalar_array_add_vec(&*_c_tMaxMin, &*_at, num_sol);

	for(j=3; j<=4; j++)
	{
		memcpy(&*_at, &*at, num_sol*sizeof(double));
		
		scalar_array_pow(&*_at, (real_t)(j)-(real_t)(1.0), num_sol);
		scalar_array_mult(&*_at, _a[j], num_sol);
		scalar_array_mult(&*_at, (real_t)(j), num_sol);
		scalar_array_add_vec(&*_c_tMaxMin, &*_at, num_sol);
	}

	double tMaxMin[num_sol];
	double al_[num_sol];
	int al_idx = 0, a;
	
	memcpy(&*tMaxMin, &*_c_tMaxMin, num_sol*sizeof(double));
	
	for(i=0; i<num_sol; i++)
	{
		if(tMaxMin[i] > 0) al_[al_idx++] = al[i];
	}
	
	for(a=0; a<al_idx; a++)
	{
		vec3_t rpy;
		vec3_assign(&rpy, (real_t)0, (real_t)(al_[a] * CONST_PI / (real_t)(180)), (real_t)(0));
		mat33_t R, Ry_;
		rpyMat(&Ry_, &rpy);
		mat33_mult_mat2(&R, Rz, &Ry_);
		vec3_t t_opt;
		vec3_clear(&t_opt);

		for(i=0; i<n; i++)
		{
			mat33_t _m1, _eye3;
			mat33_eye(&_eye3);
			mat33_copy(&_m1, &V[i]);
			mat33_sub(&_m1, &_eye3);
			vec3_t _v1, _v2;
			vec3_mult_mat(&_v1, &R, &p[i]);
			vec3_mult_mat(&_v2, &_m1, &_v1);
			vec3_add_vec(&t_opt, &_v2);
		}

		vec3_t t_opt_;
		vec3_mult_mat(&t_opt_, &G, &t_opt);
		tnew[a] = t_opt_;
	}
	
	memcpy(al_ret, &*al_, al_idx*sizeof(double));
	
	*res_n = al_idx;
}
void arGetInitRot2_sub(rpp_float &err,
					   rpp_mat &R,
					   rpp_vec &t,
					   const rpp_float cc[2],
					   const rpp_float fc[2],
					   const rpp_vec *model,
					   const rpp_vec *iprts,
					   const unsigned int model_iprts_size,
					   const rpp_mat R_init,
					   const bool estimate_R_init,
					   const rpp_float epsilon,
					   const rpp_float tolerance,
					   const unsigned int max_iterations)
{
	vec3_array _model;
	vec3_array _iprts;
	_model.resize(model_iprts_size);
	_iprts.resize(model_iprts_size);

	mat33_t K, K_inv;
	mat33_eye(K);
	K.m[0][0] = (real_t)fc[0];
	K.m[1][1] = (real_t)fc[1];
	K.m[0][2] = (real_t)cc[0];
	K.m[1][2] = (real_t)cc[1];

	mat33_inv(K_inv, K);

	for(unsigned int i=0; i<model_iprts_size; i++)
	{
		vec3_t _v,_v2;
		vec3_assign(_v,(real_t)model[i][0],(real_t)model[i][1],(real_t)model[i][2]);
		_model[i] = _v;
		vec3_assign(_v,(real_t)iprts[i][0],(real_t)iprts[i][1],(real_t)iprts[i][2]);
		vec3_mult(_v2,K_inv,_v);
		_iprts[i] = _v2;
	}

	options_t options;
	options.max_iter = max_iterations;
	options.epsilon = (real_t)(epsilon == 0 ? DEFAULT_EPSILON : epsilon);
	options.tol =     (real_t)(tolerance == 0 ? DEFAULT_TOL : tolerance);
	if(estimate_R_init)
		mat33_set_all_zeros(options.initR);
	else
	{
		mat33_assign(options.initR,
			(real_t)R_init[0][0], (real_t)R_init[0][1], (real_t)R_init[0][2],
			(real_t)R_init[1][0], (real_t)R_init[1][1], (real_t)R_init[1][2],
			(real_t)R_init[2][0], (real_t)R_init[2][1], (real_t)R_init[2][2]);
	}

	real_t _err;
	mat33_t _R;
	vec3_t _t;

	arGetInitRot2_sub2(_err,_R,_t,_model,_iprts,options);

	for(int j=0; j<3; j++)
	{
		R[j][0] = (rpp_float)_R.m[j][0];
		R[j][1] = (rpp_float)_R.m[j][1];
		R[j][2] = (rpp_float)_R.m[j][2];
		t[j] = (rpp_float)_t.v[j];
	}
	err = (rpp_float)_err;
}
示例#18
0
文件: vec3.c 项目: Raphy42/RT
/**
 * return src / src.length
 */
inline t_vec3      *vec3_unit_vector(t_vec3 *v, const t_vec3 *src)
{
    return (vec3_assign(v, vec3_div_f(v, src, vec3_length(src))));
}