Vector3 refract_dir(Vector3 d, Vector3 n, Real eta)
{
Real c1, c2;
if ((c1 = v3_dot(d, n)) < 0)
c1 = -c1;
else
n = v3_scale(-1.0, n);
if ((c2 = 1 - SQR(eta) * (1 - SQR(c1))) < 0)
return v3_make(0,0,0);
else
return v3_add(v3_scale(eta, d), v3_scale(eta*c1 - sqrt(c2), n));
}
int main(int argc, char **argv)
{
Poly *l, *p, *c = poly_alloc(3);
Item *i;
init_sdl();
s = scene_read();
init_render();
for (o = s->objs; o != NULL; o = o->next) {
for (l = prim_uv_decomp(o->u.prim, 1.); l != NULL; l = l->next) {
p = poly_transform(prim_polys(o->u.prim, l), mclip);
if (!is_backfacing(p, v3_unit(v3_scale(-1, poly_centr(p)))))
hither_clip(VIEW_ZMIN(s->view), p, z_store, plist_free);
}
}
z = z_sort(z);
for (i = z->head; i != NULL; i = i->next) {
gouraud_shade(c, P(i), N(i), s->view->center, rc, M(i));
p = poly_homoxform(S(i),mdpy);
scan_poly(p, gouraud_paint, gouraud_set(g,c,s->img));
}
img_write(s->img, "stdout", 0);
exit(0);
}
vec3_t get_mouse_hit(float x, float y)
{
double mv[16], proj[16];
int vp[4];
double res_x, res_y, res_z;
float t;
vec3_t res, pnear, pfar;
glGetDoublev(GL_MODELVIEW_MATRIX, mv);
glGetDoublev(GL_PROJECTION_MATRIX, proj);
glGetIntegerv(GL_VIEWPORT, vp);
y = vp[3] - y;
gluUnProject(x, y, 0, mv, proj, vp, &res_x, &res_y, &res_z);
pnear.x = res_x;
pnear.y = res_y;
pnear.z = res_z;
gluUnProject(x, y, 1, mv, proj, vp, &res_x, &res_y, &res_z);
pfar.x = res_x;
pfar.y = res_y;
pfar.z = res_z;
t = fabs(pnear.z) / fabs(pfar.z - pnear.z);
res = v3_add(pnear, v3_scale(v3_sub(pfar, pnear), t));
return res;
}
int main(int argc, char **argv)
{
Poly *l, *p, *q = poly_alloc(3);
Hpoly *t = hpoly_alloc(3);
Item *i;
init_sdl();
s = scene_read();
init_render();
for (o = s->objs; o != NULL; o = o->next) {
for (l = prim_uv_decomp(o->u.prim, 1.); l != NULL; l = l->next) {
p = poly_transform(prim_polys(o->u.prim, l), mclip);
if (!is_backfacing(p, v3_unit(v3_scale(-1, poly_centr(p)))))
hither_clip(0, p, z_store, plist_free);
}
}
z = z_sort(z);
for (i = z->head; i != NULL; i = i->next) {
t = hpoly_polyxform(t, S(i), mdpy);
q = poly_wz_hpoly(q, W(i), t);
texture_wscale(W(i), T(i));
scan_spoly3(q, 2, texture_shadepaint,
texture_set(td,W(i),T(i),P(i),N(i),DU(i),DV(i),rc,M(i)));
}
img_write(s->img, "stdout", 0);
exit(0);
}
Sprite* sprite_create(const char* texture_path) {
Sprite* sprite = malloc(sizeof(*sprite));
if(sprite == NULL) {
perror("Sprite creation");
return NULL;
}
strcpy(sprite->path, texture_path);
sprite->texture = texture_load(sprite->path);
sprite->width = texture_get_param(sprite->texture, GL_TEXTURE_WIDTH);
sprite->height = texture_get_param(sprite->texture, GL_TEXTURE_HEIGHT);
sprite->transform = m4_identity();
vec3 vertices[num_vertices];
vec3 scale = {sprite->width, sprite->height, 1};
int i;
for(i = 0; i < num_vertices; ++i) {
v3_scale(&vertices[i], &quad_vertices[i], &scale);
}
sprite->attributes[0].buffer = buffer_create(&vertices, sizeof(vertices));
sprite->attributes[0].size = 3;
sprite->attributes[1].buffer = buffer_create(&quad_uv, sizeof(quad_uv));
sprite->attributes[1].size = 2;
return sprite;
}
int dir_coupling(Cone a, Vector3 v)
{
if (v3_dot(a.d, v3_scale(-1, v)) > a.cosa)
return TRUE;
else
return FALSE;
}
Color bump_map(void *info, Vector3 t, Vector3 n, Vector3 ds, Vector3 dt)
{
TextureSrc *src = info;
Real h = 0.0005;
Real fo = texture_c1((*src->texfunc)(src->texdata, t));
Real fu = texture_c1((*src->texfunc)(src->texdata, v3_add(t,v3_make(h,0,0))));
Real fv = texture_c1((*src->texfunc)(src->texdata, v3_add(t,v3_make(0,h,0))));
Real du = fderiv(fo, fu, h);
Real dv = fderiv(fo, fv, h);
Vector3 u = v3_scale(du, v3_cross(n, dt));
Vector3 v = v3_scale(-dv, v3_cross(n, ds));
return v3_add(u, v);
}
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);
}
/* Compute the mean of a set of vectors */
v3_t v3_mean(int n, const v3_t *v) {
int i;
v3_t mean = v3_new(0.0, 0.0, 0.0);
for (i = 0; i < n; i++) {
mean = v3_add(mean, v[i]);
}
return v3_scale(1.0 / n, mean);
}
static void tri_refine(Vector3 v0, Vector3 v1, Vector3 v2, Real s, int maxrec)
{
Real l1 = v3_norm(v3_sub(v0, v1));
Real l2 = v3_norm(v3_sub(v1, v2));
Real l3 = v3_norm(v3_sub(v2, v0));
if ((MAX(l1, MAX(l2, l3))) <= s || --maxrec < 0) {
plist = poly_insert(plist, poly3_make(v0, v1, v2));
} else {
Vector3 m1 = v3_scale(0.5, v3_add(v0, v1));
Vector3 m2 = v3_scale(0.5, v3_add(v1, v2));
Vector3 m3 = v3_scale(0.5, v3_add(v2, v0));
tri_refine(m1, v1, m2, s, maxrec);
tri_refine(m2, v2, m3, s, maxrec);
tri_refine(m3, v0, m1, s, maxrec);
tri_refine(m1, m2, m3, s, maxrec);
}
}
void makeviewVi(void)
{
Vector3 n,u,v,t;
n = view->normal;
v = v3_sub(view->up, v3_scale(v3_dot(view->up, n), n));
if (v3_norm(v) < ROUNDOFF)
error("view up parallel to view normal");
v = v3_unit(v);
u = v3_cross(n, v);
v = v3_cross(u, n);
n = v3_scale(-1.0, n);
t = view->center;
view->Vinv = m4_ident();
view->Vinv.r1.x = u.x; view->Vinv.r2.x = u.y; view->Vinv.r3.x = u.z;
view->Vinv.r1.y = v.x; view->Vinv.r2.y = v.y; view->Vinv.r3.y = v.z;
view->Vinv.r1.z = n.x; view->Vinv.r2.z = n.y; view->Vinv.r3.z = n.z;
view->Vinv.r1.w = t.x; view->Vinv.r2.w = t.y; view->Vinv.r3.w = t.z;
}
void subdiv_line(Vector3 p, Vector3 q, void (*paint)())
{
Box3d bb = bound(p, q);
if ((bb.ur.x - bb.ll.x) <= 1 && (bb.ur.y - bb.ll.y) <= 1) {
paint(bb.ll.x, bb.ll.y);
} else {
Vector3 m = v3_scale(0.5, v3_add(p,q));
subdiv_line(p, m, paint);
subdiv_line(m, q, paint);
}
}
static Real formfactor(int i, int j, int n, Poly **p, Real *a)
{
Vector3 vi, vj, vji, d;
Real r2, ci, cj;
vi = poly_centr(p[i]);
vj = poly_centr(p[j]);
vji = v3_sub(vi, vj);
if ((r2 = v3_sqrnorm(vji)) < REL_EPS)
return 0;
d = v3_scale(1.0/sqrt(r2), vji);
if ((cj = v3_dot(poly_normal(p[j]), d)) < REL_EPS)
return 0;
if ((ci = -v3_dot(poly_normal(p[i]), d)) < REL_EPS)
return 0;
if (vis_flag && visible(n, p, vj, vji) < REL_EPS)
return 0;
return a[i] * ((cj * ci) / (PI * r2 + a[i]));
}
/* Assumes unit_normal is normalized */
v3_t v3_project(const v3_t v, const v3_t unit_normal) {
double dot = v3_dotp(v, unit_normal);
v3_t par = v3_scale(dot, unit_normal);
v3_t perp = v3_sub(v, par);
return perp;
}
void GLWidget::keyPressEvent(QKeyEvent *event)
{
Matrix4 M;
Vector3 Dir;
switch (event->key()) {
case Qt::Key_Q:
case Qt::Key_A:
M = m4_rotate('y', 0.5);
scene->view->center = v3_m4mult(scene->view->center,M);
scene->view->normal = v3_m4mult(scene->view->normal,M);
makeviewV();
setview(scene->view);
break;
case Qt::Key_D:
M = m4_rotate('y', -0.5);
scene->view->center = v3_m4mult(scene->view->center,M);
scene->view->normal = v3_m4mult(scene->view->normal,M);
makeviewV();
setview(scene->view);
break;
case Qt::Key_W:
M = m4_rotate('x', 0.5);
scene->view->center = v3_m4mult(scene->view->center,M);
scene->view->up = v3_m4mult(scene->view->up,M);
scene->view->normal = v3_m4mult(scene->view->normal,M);
makeviewV();
setview(scene->view);
break;
case Qt::Key_S:
M = m4_rotate('x', -0.5);
scene->view->center = v3_m4mult(scene->view->center,M);
scene->view->up = v3_m4mult(scene->view->up,M);
scene->view->normal = v3_m4mult(scene->view->normal,M);
makeviewV();
setview(scene->view);
break;
case Qt::Key_Left:
Dir = v3_cross(scene->view->up,scene->view->normal);
scene->view->center = v3_add(scene->view->center,v3_scale(0.5,Dir));
makeviewV();
setview(scene->view);
break;
case Qt::Key_Right:
Dir = v3_cross(scene->view->up,scene->view->normal);
scene->view->center = v3_sub(scene->view->center,v3_scale(0.5,Dir));
makeviewV();
setview(scene->view);
break;
case Qt::Key_Up:
scene->view->center = v3_add(scene->view->center,v3_scale(0.5,scene->view->up));
makeviewV();
setview(scene->view);
break;
case Qt::Key_Down:
scene->view->center = v3_sub(scene->view->center,v3_scale(0.5,scene->view->up));
makeviewV();
setview(scene->view);
break;
case Qt::Key_Escape:
exit(0);
break;
case Qt::Key_R:
if (!lastfile.isEmpty())
load_scene_file(lastfile.toLatin1().data());
break;
}
update();
}
/* Computes the closed-form least-squares solution to a rigid
* body alignment.
*
* n: the number of points
* right_pts: Target set of n points
* left_pts: Source set of n points */
double align_horn_3D(int n, v3_t *right_pts, v3_t *left_pts, int scale_xform,
double *Tout) {
int i;
v3_t right_centroid = v3_new(0.0, 0.0, 0.0);
v3_t left_centroid = v3_new(0.0, 0.0, 0.0);
double M[3][3] = { { 0.0, 0.0, 0.0, },
{ 0.0, 0.0, 0.0, },
{ 0.0, 0.0, 0.0, } };
double MT[3][3];
double MTM[3][3];
double eval[3], sqrteval_inv[3];
double evec[3][3], evec_tmp[3][3];
double Sinv[3][3], U[3][3];
double Tcenter[4][4] = { { 1.0, 0.0, 0.0, 0.0 },
{ 0.0, 1.0, 0.0, 0.0 },
{ 0.0, 0.0, 1.0, 0.0 },
{ 0.0, 0.0, 0.0, 1.0 } };
double Ttmp[4][4];
double T[16], R[16];
double sum_num, sum_den, scale, RMS_sum;
int perm[3];
/* Compute the centroid of both point sets */
right_centroid = v3_mean(n, right_pts);
left_centroid = v3_mean(n, left_pts);
/* Compute the scale */
sum_num = sum_den = 0.0;
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
sum_num += v3_magsq(r);
sum_den += v3_magsq(l);
}
scale = sqrt(sum_num / sum_den);
/* Fill in the matrix M */
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
M[0][0] += Vx(r) * Vx(l);
M[0][1] += Vx(r) * Vy(l);
M[0][2] += Vx(r) * Vz(l);
M[1][0] += Vy(r) * Vx(l);
M[1][1] += Vy(r) * Vy(l);
M[1][2] += Vy(r) * Vz(l);
M[2][0] += Vz(r) * Vx(l);
M[2][1] += Vz(r) * Vy(l);
M[2][2] += Vz(r) * Vz(l);
}
/* Compute MTM */
matrix_transpose(3, 3, (double *)M, (double *)MT);
matrix_product(3, 3, 3, 3, (double *)MT, (double *)M, (double *)MTM);
/* Calculate Sinv, the inverse of the square root of MTM */
dgeev_driver(3, (double *)MTM, (double *)evec, eval);
/* Sort the eigenvalues */
qsort_descending();
qsort_perm(3, eval, perm);
memcpy(evec_tmp[0], evec[perm[0]], sizeof(double) * 3);
memcpy(evec_tmp[1], evec[perm[1]], sizeof(double) * 3);
memcpy(evec_tmp[2], evec[perm[2]], sizeof(double) * 3);
memcpy(evec, evec_tmp, sizeof(double) * 9);
sqrteval_inv[0] = 1.0 / sqrt(eval[0]);
sqrteval_inv[1] = 1.0 / sqrt(eval[1]);
if (eval[2] < 1.0e-8 * eval[0]) {
sqrteval_inv[2] = 0.0;
} else {
sqrteval_inv[2] = 1.0 / sqrt(eval[2]);
}
Sinv[0][0] =
sqrteval_inv[0] * evec[0][0] * evec[0][0] +
sqrteval_inv[1] * evec[1][0] * evec[1][0] +
sqrteval_inv[2] * evec[2][0] * evec[2][0];
Sinv[0][1] =
sqrteval_inv[0] * evec[0][0] * evec[0][1] +
sqrteval_inv[1] * evec[1][0] * evec[1][1] +
sqrteval_inv[2] * evec[2][0] * evec[2][1];
Sinv[0][2] =
sqrteval_inv[0] * evec[0][0] * evec[0][2] +
sqrteval_inv[1] * evec[1][0] * evec[1][2] +
sqrteval_inv[2] * evec[2][0] * evec[2][2];
Sinv[1][0] =
sqrteval_inv[0] * evec[0][1] * evec[0][0] +
sqrteval_inv[1] * evec[1][1] * evec[1][0] +
sqrteval_inv[2] * evec[2][1] * evec[2][0];
Sinv[1][1] =
sqrteval_inv[0] * evec[0][1] * evec[0][1] +
sqrteval_inv[1] * evec[1][1] * evec[1][1] +
sqrteval_inv[2] * evec[2][1] * evec[2][1];
Sinv[1][2] =
sqrteval_inv[0] * evec[0][1] * evec[0][2] +
sqrteval_inv[1] * evec[1][1] * evec[1][2] +
sqrteval_inv[2] * evec[2][1] * evec[2][2];
Sinv[2][0] =
sqrteval_inv[0] * evec[0][2] * evec[0][0] +
sqrteval_inv[1] * evec[1][2] * evec[1][0] +
sqrteval_inv[2] * evec[2][2] * evec[2][0];
Sinv[2][1] =
sqrteval_inv[0] * evec[0][2] * evec[0][1] +
sqrteval_inv[1] * evec[1][2] * evec[1][1] +
sqrteval_inv[2] * evec[2][2] * evec[2][1];
Sinv[2][2] =
sqrteval_inv[0] * evec[0][2] * evec[0][2] +
sqrteval_inv[1] * evec[1][2] * evec[1][2] +
sqrteval_inv[2] * evec[2][2] * evec[2][2];
/* U = M * Sinv */
matrix_product(3, 3, 3, 3, (double *)M, (double *)Sinv, (double *)U);
if (eval[2] < 1.0e-8 * eval[0]) {
double u3u3[9], Utmp[9];
matrix_transpose_product2(3, 1, 3, 1, evec[2], evec[2], u3u3);
matrix_sum(3, 3, 3, 3, (double *) U, u3u3, Utmp);
if (matrix_determinant3(Utmp) < 0.0) {
printf("[align_horn_3D] Recomputing matrix...\n");
matrix_diff(3, 3, 3, 3, (double *) U, u3u3, Utmp);
}
memcpy(U, Utmp, 9 * sizeof(double));
}
/* Fill in the rotation matrix */
R[0] = U[0][0]; R[1] = U[0][1]; R[2] = U[0][2]; R[3] = 0.0;
R[4] = U[1][0]; R[5] = U[1][1]; R[6] = U[1][2]; R[7] = 0.0;
R[8] = U[2][0]; R[9] = U[2][1]; R[10] = U[2][2]; R[11] = 0.0;
R[12] = 0.0; R[13] = 0.0; R[14] = 0.0; R[15] = 1.0;
/* Fill in the translation matrix */
matrix_ident(4, T);
T[3] = Vx(right_centroid);
T[7] = Vy(right_centroid);
T[11] = Vz(right_centroid);
if (scale_xform == 0)
scale = 1.0;
Tcenter[0][0] = scale;
Tcenter[1][1] = scale;
Tcenter[2][2] = scale;
Tcenter[0][3] = -scale * Vx(left_centroid);
Tcenter[1][3] = -scale * Vy(left_centroid);
Tcenter[2][3] = -scale * Vz(left_centroid);
matrix_product(4, 4, 4, 4, T, R, (double *) Ttmp);
matrix_product(4, 4, 4, 4, (double *)Ttmp, (double *)Tcenter, Tout);
#if 0
T[2] = Vx(v3_sub(right_centroid, left_centroid));
T[5] = Vy(v3_sub(right_centroid, left_centroid));
T[8] = Vz(v3_sub(right_centroid, left_centroid));
#endif
/* Now compute the RMS error between the points */
RMS_sum = 0.0;
for (i = 0; i < n; i++) {
double left[4] = { Vx(left_pts[i]),
Vy(left_pts[i]),
Vz(left_pts[i]), 1.0 };
double left_prime[3];
double dx, dy, dz;
matrix_product(4, 4, 4, 1, Tout, left, left_prime);
dx = left_prime[0] - Vx(right_pts[i]);
dy = left_prime[1] - Vy(right_pts[i]);
dz = left_prime[2] - Vz(right_pts[i]);
RMS_sum += dx * dx + dy * dy + dz * dz;
#if 0
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
v3_t resid;
/* Rotate, scale l */
v3_t Rl, SRl;
Vx(Rl) = R[0] * Vx(l) + R[1] * Vy(l) + R[2] * Vz(l);
Vy(Rl) = R[3] * Vx(l) + R[4] * Vy(l) + R[5] * Vz(l);
Vz(Rl) = R[6] * Vx(l) + R[7] * Vy(l) + R[8] * Vz(l);
SRl = v3_scale(scale, Rl);
resid = v3_sub(r, SRl);
RMS_sum += v3_magsq(resid);
#endif
}
return sqrt(RMS_sum / n);
}
/* Computes the closed-form least-squares solution to a rigid
* body alignment.
*
* n: the number of points
* right_pts: Target set of n points
* left_pts: Source set of n points */
double align_horn(int n, v3_t *right_pts, v3_t *left_pts,
double *R, double *T,
double *Tout, double *scale, double *weight) {
int i;
v3_t right_centroid = v3_new(0.0, 0.0, 0.0);
v3_t left_centroid = v3_new(0.0, 0.0, 0.0);
double M[2][2] = { { 0.0, 0.0 },
{ 0.0, 0.0 } };
double MT[2][2];
double MTM[2][2];
double eval[2], sqrteval[2];
double evec[2][2];
double S[2][2], Sinv[2][2], U[2][2];
double Tcenter[3][3] = { { 1.0, 0.0, 0.0 },
{ 0.0, 1.0, 0.0 },
{ 0.0, 0.0, 1.0 } };
double Ttmp[3][3];
double sum_num, sum_den, RMS_sum;
#if 1
double weight_sum = 0.0;
if (weight == NULL) {
weight_sum = n;
for (i = 0; i < n; i++) {
right_centroid =
v3_add(right_centroid, right_pts[i]);
left_centroid =
v3_add(left_centroid, left_pts[i]);
}
right_centroid = v3_scale(1.0 / weight_sum, right_centroid);
left_centroid = v3_scale(1.0 / weight_sum, left_centroid);
} else {
/* Compute the weighted centroid of both point sets */
for (i = 0; i < n; i++) {
right_centroid =
v3_add(right_centroid, v3_scale(weight[i], right_pts[i]));
left_centroid =
v3_add(left_centroid, v3_scale(weight[i], left_pts[i]));
weight_sum += weight[i];
}
right_centroid = v3_scale(1.0 / weight_sum, right_centroid);
left_centroid = v3_scale(1.0 / weight_sum, left_centroid);
}
#else
/* Calculate the centroid of both sets of points */
for (i = 0; i < n; i++) {
right_centroid = v3_add(right_centroid, right_pts[i]);
left_centroid = v3_add(left_centroid, left_pts[i]);
}
right_centroid = v3_scale(1.0 / n, right_centroid);
left_centroid = v3_scale(1.0 / n, left_centroid);
#endif
/* Compute the scale */
sum_num = sum_den = 0.0;
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
sum_num = v3_magsq(r);
sum_den = v3_magsq(l);
}
*scale = sqrt(sum_num / sum_den);
/* Fill in the matrix M */
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
if (weight != NULL) {
M[0][0] += Vx(r) * Vx(l);
M[0][1] += Vx(r) * Vy(l);
M[1][0] += Vy(r) * Vx(l);
M[1][1] += Vy(r) * Vy(l);
} else {
M[0][0] += Vx(r) * Vx(l);
M[0][1] += Vx(r) * Vy(l);
M[1][0] += Vy(r) * Vx(l);
M[1][1] += Vy(r) * Vy(l);
}
}
/* Compute MTM */
matrix_transpose(2, 2, (double *)M, (double *)MT);
matrix_product(2, 2, 2, 2, (double *)MT, (double *)M, (double *)MTM);
/* Calculate Sinv, the inverse of the square root of MTM */
dgeev_driver(2, (double *)MTM, (double *)evec, eval);
/* MTM = eval[0] * evec[0]T * evec[0] + eval[1] * evec[1]T * evec[1] */
/* S = sqrt(eval[0]) * evec[0]T * evec[0] + sqrt(eval[1]) * evec[1]T * evec[1] */
sqrteval[0] = sqrt(eval[0]);
sqrteval[1] = sqrt(eval[1]);
S[0][0] =
(sqrteval[0]) * evec[0][0] * evec[0][0] +
(sqrteval[1]) * evec[1][0] * evec[1][0];
S[0][1] =
(sqrteval[0]) * evec[0][0] * evec[0][1] +
(sqrteval[1]) * evec[1][0] * evec[1][1];
S[1][0] =
(sqrteval[0]) * evec[0][1] * evec[0][0] +
(sqrteval[1]) * evec[1][1] * evec[1][0];
S[1][1] =
(sqrteval[0]) * evec[0][1] * evec[0][1] +
(sqrteval[1]) * evec[1][1] * evec[1][1];
Sinv[0][0] =
(1.0 / sqrteval[0]) * evec[0][0] * evec[0][0] +
(1.0 / sqrteval[1]) * evec[1][0] * evec[1][0];
Sinv[0][1] =
(1.0 / sqrteval[0]) * evec[0][0] * evec[0][1] +
(1.0 / sqrteval[1]) * evec[1][0] * evec[1][1];
Sinv[1][0] =
(1.0 / sqrteval[0]) * evec[0][1] * evec[0][0] +
(1.0 / sqrteval[1]) * evec[1][1] * evec[1][0];
Sinv[1][1] =
(1.0 / sqrteval[0]) * evec[0][1] * evec[0][1] +
(1.0 / sqrteval[1]) * evec[1][1] * evec[1][1];
// matrix_product(2, 2, 2, 2, (double *)S, (double *)Sinv, (double *)U);
/* U = M * Sinv */
matrix_product(2, 2, 2, 2, (double *)M, (double *)Sinv, (double *)U);
/* Fill in the rotation matrix */
R[0] = U[0][0]; R[1] = U[0][1]; R[2] = 0.0;
R[3] = U[1][0], R[4] = U[1][1]; R[5] = 0.0;
R[6] = 0.0; R[7] = 0.0; R[8] = 1.0;
// memcpy(R, U, sizeof(double) * 4);
/* Fill in the translation matrix */
T[0] = T[4] = T[8] = 1.0;
T[1] = T[3] = T[6] = T[7] = 0.0;
T[2] = Vx(right_centroid);
T[5] = Vy(right_centroid);
Tcenter[0][0] = *scale;
Tcenter[1][1] = *scale;
Tcenter[0][2] = -*scale * Vx(left_centroid);
Tcenter[1][2] = -*scale * Vy(left_centroid);
matrix_product(3, 3, 3, 3, T, R, (double *)Ttmp);
#if 0
#if 0
/* Do the scaling */
Ttmp[0][0] *= *scale;
Ttmp[0][1] *= *scale;
Ttmp[0][2] *= *scale;
Ttmp[1][0] *= *scale;
Ttmp[1][1] *= *scale;
Ttmp[1][2] *= *scale;
#else
Tcenter[0][0] *= *scale;
Tcenter[0][2] *= *scale;
Tcenter[1][1] *= *scale;
Tcenter[1][2] *= *scale;
#endif
#endif
matrix_product(3, 3, 3, 3, (double *)Ttmp, (double *)Tcenter, Tout);
T[2] = Vx(v3_sub(right_centroid, left_centroid));
T[5] = Vy(v3_sub(right_centroid, left_centroid));
/* Now compute the RMS error between the points */
RMS_sum = 0.0;
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
v3_t resid;
/* Rotate, scale l */
v3_t Rl, SRl;
Vx(Rl) = R[0] * Vx(l) + R[1] * Vy(l) + R[2] * Vz(l);
Vy(Rl) = R[3] * Vx(l) + R[4] * Vy(l) + R[5] * Vz(l);
Vz(Rl) = R[6] * Vx(l) + R[7] * Vy(l) + R[8] * Vz(l);
SRl = v3_scale(*scale, Rl);
resid = v3_sub(r, SRl);
RMS_sum += v3_magsq(resid);
}
return sqrt(RMS_sum / n);
}
/* Computes the closed-form least-squares solution to a rigid
* body alignment.
*
* n: the number of points
* right_pts: Target set of n points
* left_pts: Source set of n points */
double align_horn_3D_2(int n, v3_t *right_pts, v3_t *left_pts, int scale_xform,
double *Tout)
{
int i;
v3_t right_centroid = v3_new(0.0, 0.0, 0.0);
v3_t left_centroid = v3_new(0.0, 0.0, 0.0);
double Tcenter[16] = { 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0 };
double Ttmp[4][4];
double T[16], R[16], R3x3[9];
double sum_num, sum_den, scale, RMS_sum;
v3_t *left_pts_zm = malloc(sizeof(v3_t) * n);
v3_t *right_pts_zm = malloc(sizeof(v3_t) * n);
double error = 0.0;
/* Compute the centroid of both point sets */
right_centroid = v3_mean(n, right_pts);
left_centroid = v3_mean(n, left_pts);
/* Compute the scale */
sum_num = sum_den = 0.0;
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
sum_num += v3_magsq(r);
sum_den += v3_magsq(l);
}
scale = sqrt(sum_num / sum_den);
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
right_pts_zm[i] = r;
left_pts_zm[i] = v3_scale(scale, l);
}
/* Compute the rotation */
error = align_3D_rotation(n, right_pts_zm, left_pts_zm, R3x3);
// printf("error[%d]: %0.3f\n", n, error);
// matrix_print(3, 3, R3x3);
/* Fill in the rotation matrix */
R[0] = R3x3[0]; R[1] = R3x3[1]; R[2] = R3x3[2]; R[3] = 0.0;
R[4] = R3x3[3]; R[5] = R3x3[4]; R[6] = R3x3[5]; R[7] = 0.0;
R[8] = R3x3[6]; R[9] = R3x3[7]; R[10] = R3x3[8]; R[11] = 0.0;
R[12] = 0.0; R[13] = 0.0; R[14] = 0.0; R[15] = 1.0;
/* Fill in the translation matrix */
// matrix_ident(4, T);
T[0] = 1.0; T[1] = 0.0; T[2] = 0.0; T[3] = Vx(right_centroid);
T[4] = 0.0; T[5] = 1.0; T[6] = 0.0; T[7] = Vy(right_centroid);
T[8] = 0.0; T[9] = 0.0; T[10] = 1.0; T[11] = Vz(right_centroid);
T[12] = 0.0; T[13] = 0.0; T[14] = 0.0; T[15] = 1.0;
if (scale_xform == 0)
scale = 1.0;
Tcenter[0] = scale;
Tcenter[5] = scale;
Tcenter[10] = scale;
Tcenter[3] = -scale * Vx(left_centroid);
Tcenter[7] = -scale * Vy(left_centroid);
Tcenter[11] = -scale * Vz(left_centroid);
matrix_product44(T, R, (double *) Ttmp);
matrix_product44((double *)Ttmp, (double *)Tcenter, Tout);
/* Now compute the RMS error between the points */
RMS_sum = 0.0;
for (i = 0; i < n; i++) {
double left[4] = { Vx(left_pts[i]),
Vy(left_pts[i]),
Vz(left_pts[i]), 1.0 };
double left_prime[3];
double dx, dy, dz;
matrix_product441(Tout, left, left_prime);
dx = left_prime[0] - Vx(right_pts[i]);
dy = left_prime[1] - Vy(right_pts[i]);
dz = left_prime[2] - Vz(right_pts[i]);
RMS_sum += dx * dx + dy * dy + dz * dz;
}
free(left_pts_zm);
free(right_pts_zm);
return sqrt(RMS_sum / n);
}
/* Align two sets of points with a 2D similarity transform */
int align_2D_ransac(int n, v3_t *r_pts, v3_t *l_pts,
int num_ransac_rounds, double ransac_thresh,
double *Tret)
{
int round;
#define MIN_SUPPORT 2
v3_t *l_inliers, *r_inliers;
int num_inliers, max_inliers = 0;
double Tbest[9], R[9], T[9], scale;
int i;
if (n < 3) {
printf("[align_2D_ransac] Error: need at least 3 points!\n");
return 0;
}
l_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
r_inliers = (v3_t *) malloc(sizeof(v3_t) * n);
for (round = 0; round < num_ransac_rounds; round++) {
int support[MIN_SUPPORT];
int i, j;
v3_t r_mean, l_mean, r0, l0;
v3_t r_pts_small[MIN_SUPPORT], l_pts_small[MIN_SUPPORT];
double Rtmp[9], T1tmp[9], T2tmp[9], tmp[9], Tout[9];
double a, b;
for (i = 0; i < MIN_SUPPORT; i++) {
/* Select an index from 0 to n-1 */
int idx, reselect;
do {
reselect = 0;
idx = rand() % n;
for (j = 0; j < i; j++) {
if (support[j] == idx) {
reselect = 1;
break;
}
}
} while (reselect);
support[i] = idx;
r_pts_small[i] = r_pts[idx];
l_pts_small[i] = l_pts[idx];
}
r_mean = v3_scale(0.5, v3_add(r_pts_small[0], r_pts_small[1]));
l_mean = v3_scale(0.5, v3_add(l_pts_small[0], l_pts_small[1]));
r0 = v3_sub(r_pts_small[0], r_mean);
// v3_t r1 = v3_sub(r_pts_small[1], mean);
l0 = v3_sub(l_pts_small[0], l_mean);
// v3_t l1 = v3_sub(l_pts_small[1], mean);
a = (Vy(r0) + Vx(r0) * Vx(l0) / Vy(l0)) /
(Vy(l0) + Vx(l0) * Vx(l0) / Vy(l0));
b = (Vx(r0) - a * Vx(l0)) / Vy(l0);
Rtmp[0] = a; Rtmp[1] = b; Rtmp[2] = 0.0;
Rtmp[3] = -b; Rtmp[4] = a; Rtmp[5] = 0.0;
Rtmp[6] = 0; Rtmp[7] = 0; Rtmp[8] = 1.0;
matrix_ident(3, T1tmp);
T1tmp[2] = -Vx(l_mean);
T1tmp[5] = -Vy(l_mean);
matrix_ident(3, T2tmp);
T2tmp[2] = Vx(r_mean);
T2tmp[5] = Vy(r_mean);
matrix_product(3, 3, 3, 3, Rtmp, T1tmp, tmp);
matrix_product(3, 3, 3, 3, T2tmp, tmp, Tout);
/* Count inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double Tp[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Tout, l_pts[i].p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
num_inliers++;
}
if (num_inliers > max_inliers) {
max_inliers = num_inliers;
memcpy(Tbest, Tout, sizeof(double) * 9);
// printf(" inliers_new: %d\n", num_inliers);
}
}
}
#if 0
/* Reestimate using all inliers */
num_inliers = 0;
for (i = 0; i < n; i++) {
double Tp[3];
double diff[3];
double dist;
matrix_product(3, 3, 3, 1, Tbest, l_pts[i].p, Tp);
matrix_diff(3, 1, 3, 1, Tp, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
if (dist < ransac_thresh) {
r_inliers[num_inliers] = r_pts[i];
l_inliers[num_inliers] = l_pts[i];
num_inliers++;
}
}
// align_horn(num_inliers, r_inliers, l_inliers, R, T, Tret, &scale, NULL);
align_2D(num_inliers, r_inliers, l_inliers, R, T, Tret, &scale, NULL);
#else
memcpy(Tret, Tbest, 9 * sizeof(double));
#endif
free(r_inliers);
free(l_inliers);
return num_inliers;
#undef MIN_SUPPORT
}
Vector3 reflect_dir(Vector3 d, Vector3 n)
{
return v3_add(d, v3_scale(-2 * v3_dot(n, d), n));
}
Vector3 cylin_gradient(Prim *p, Vector3 q)
{
Vector3 w = v3_m4mult(q, p->ti);
w.z = 0;
return v3_m3mult(v3_scale(2., w), m4_transpose(p->ti));
}
int point_coupling(Cone a, Cone b)
{
Vector3 d = v3_unit(v3_sub(a.o, b.o));
return dir_coupling(a, d) && dir_coupling(b, v3_scale(-1, d));
}