t_obj *inter2(t_scene *scene, t_vector3f d, t_vector3f o)
{
float t;
float t2;
t_objl *l;
t_obj *obj;
t_vector3f tmp[2];
l = scene->objl;
t = -1;
obj = NULL;
while (l)
{
tmp[0] = get_transforms(o, inv3(l->obj->pos), conjugate4(l->obj->rot));
tmp[1] = get_transforms(d, rgb(0, 0, 0), conjugate4(l->obj->rot));
t2 = scene->tab_type[l->obj->type](tmp[0], *l->obj, tmp[1]);
if (t2 > 0 && (t2 < t || t == -1))
{
t = t2;
obj = l->obj;
}
l = l->next;
}
if (obj && t > 0)
return (obj);
return (NULL);
}
inline mat4 inverse(mat4 const& m)
{
float coef00 = m[2][2] * m[3][3] - m[3][2] * m[2][3];
float coef02 = m[1][2] * m[3][3] - m[3][2] * m[1][3];
float coef03 = m[1][2] * m[2][3] - m[2][2] * m[1][3];
float coef04 = m[2][1] * m[3][3] - m[3][1] * m[2][3];
float coef06 = m[1][1] * m[3][3] - m[3][1] * m[1][3];
float coef07 = m[1][1] * m[2][3] - m[2][1] * m[1][3];
float coef08 = m[2][1] * m[3][2] - m[3][1] * m[2][2];
float coef10 = m[1][1] * m[3][2] - m[3][1] * m[1][2];
float coef11 = m[1][1] * m[2][2] - m[2][1] * m[1][2];
float coef12 = m[2][0] * m[3][3] - m[3][0] * m[2][3];
float coef14 = m[1][0] * m[3][3] - m[3][0] * m[1][3];
float coef15 = m[1][0] * m[2][3] - m[2][0] * m[1][3];
float coef16 = m[2][0] * m[3][2] - m[3][0] * m[2][2];
float coef18 = m[1][0] * m[3][2] - m[3][0] * m[1][2];
float coef19 = m[1][0] * m[2][2] - m[2][0] * m[1][2];
float coef20 = m[2][0] * m[3][1] - m[3][0] * m[2][1];
float coef22 = m[1][0] * m[3][1] - m[3][0] * m[1][1];
float coef23 = m[1][0] * m[2][1] - m[2][0] * m[1][1];
vec4 fac0(coef00, coef00, coef02, coef03);
vec4 fac1(coef04, coef04, coef06, coef07);
vec4 fac2(coef08, coef08, coef10, coef11);
vec4 fac3(coef12, coef12, coef14, coef15);
vec4 fac4(coef16, coef16, coef18, coef19);
vec4 fac5(coef20, coef20, coef22, coef23);
vec4 v0(m[1][0], m[0][0], m[0][0], m[0][0]);
vec4 v1(m[1][1], m[0][1], m[0][1], m[0][1]);
vec4 v2(m[1][2], m[0][2], m[0][2], m[0][2]);
vec4 v3(m[1][3], m[0][3], m[0][3], m[0][3]);
vec4 inv0(v1 * fac0 - v2 * fac1 + v3 * fac2);
vec4 inv1(v0 * fac0 - v2 * fac3 + v3 * fac4);
vec4 inv2(v0 * fac1 - v1 * fac3 + v3 * fac5);
vec4 inv3(v0 * fac2 - v1 * fac4 + v2 * fac5);
vec4 signA(+1, -1, +1, -1);
vec4 signB(-1, +1, -1, +1);
mat4 inv(inv0 * signA, inv1 * signB, inv2 * signA, inv3 * signB);
vec4 row0(inv[0][0], inv[1][0], inv[2][0], inv[3][0]);
vec4 dot0(m[0] * row0);
float dot1 = (dot0.x + dot0.y) + (dot0.z + dot0.w);
float one_over_det = 1.f / dot1;
return inv * one_over_det;
}
void add_cam(t_obj **obj, t_vector3f ray_d, t_vector3f ray_o)
{
t_camera *cam;
t_quaternion q;
cam = NULL;
q = conjugate4((*obj)->rot);
if (!(*obj)->cam)
{
if (!(cam = (t_camera *)ft_memalloc(sizeof(t_camera))))
return ;
cam->pos = get_transforms(ray_o, inv3((*obj)->pos), q);
cam->ray = get_transforms(ray_d, new_vector3f(0, 0, 0), q);
(*obj)->cam = cam;
}
else
{
(*obj)->cam->pos = get_transforms(ray_o, inv3((*obj)->pos), q);
(*obj)->cam->ray = get_transforms(ray_d, new_vector3f(0, 0, 0), q);
}
}
// Return inverse in a new matrix3
Matrix3 Matrix3::Inverse(void) const
{ // special case for 2D
if(is2D)
{ double subdet = 1./(m[0][0]*m[1][1] - m[1][0]*m[0][1]);
Matrix3 inv(m[1][1]*subdet,-m[0][1]*subdet,-m[1][0]*subdet,m[0][0]*subdet,1./m[2][2]);
return inv;
}
// general 3D
Matrix3 inv3(m[1][1]*m[2][2]-m[2][1]*m[1][2], -(m[0][1]*m[2][2]-m[2][1]*m[0][2]), m[1][0]*m[1][2]-m[1][1]*m[0][2],
-(m[1][0]*m[2][2]-m[2][0]*m[1][2]), m[0][0]*m[2][2]-m[2][0]*m[0][2], -(m[0][0]*m[1][2]-m[1][0]*m[0][2]),
m[1][0]*m[2][1]-m[2][0]*m[1][1], -(m[0][0]*m[2][1]-m[2][0]*m[0][1]), m[0][0]*m[1][1]-m[1][0]*m[0][1]);
inv3.Scale(1./determinant());
return inv3;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
mxArray const* mf1_ptr = prhs[0]; /* first moment of q(f) */
mxArray const* mf2_ptr = prhs[1]; /* raw second moment of q(f) */
mxArray const* mw1_ptr = prhs[2]; /* first moment of q(w) */
mxArray const* mw2_ptr = prhs[3]; /* raw second moment of q(w) */
mxArray const* g_ptr = prhs[4]; /* blurry image */
mxArray const* Ksharp_ptr = prhs[5]; /* intrinsic calibration of sharp image */
mxArray const* Kblurry_ptr = prhs[6]; /* Intrinsic calibration of blurry image */
mxArray const* theta_list_ptr = prhs[7]; /* angles covered by camera kernel */
mxArray const* obsmask_ptr = prhs[8]; /* Mask of observed blurry pixels */
mmu1 = mxGetScalar(prhs[9]);
mmu2 = mxGetScalar(prhs[10]);
int num_threads = (nrhs > 11) ? (int)mxGetScalar(prhs[11]) : 2;
n_kernel = mxGetN(theta_list_ptr);
mf1 = mxGetPr(mf1_ptr);
mf2 = mxGetPr(mf2_ptr);
mw1 = mxGetPr(mw1_ptr);
mw2 = mxGetPr(mw2_ptr);
g = mxGetPr(g_ptr);
Ksharp = mxGetPr(Ksharp_ptr);
Kblurry = mxGetPr(Kblurry_ptr);
theta_list = mxGetPr(theta_list_ptr);
obsmask = mxGetPr(obsmask_ptr);
if(mxGetNumberOfDimensions(mf1_ptr) > 2) mexErrMsgTxt("Images must be grayscale");
if(mxGetNumberOfDimensions(g_ptr) > 2) mexErrMsgTxt("Images must be grayscale");
int const *dims_sharp = mxGetDimensions(mf1_ptr);
h_sharp = dims_sharp[0];
w_sharp = dims_sharp[1]/2;
int const *dims_blurry = mxGetDimensions(g_ptr);
h_blurry = dims_blurry[0];
w_blurry = dims_blurry[1]/2;
n_sharp = mxGetNumberOfElements(mf1_ptr);
n_blurry = mxGetNumberOfElements(g_ptr);
/* X and Y gradients make up 2 "subimages" in the arrays, which need to be handled separately */
sharp_subim_l[0] = 1;
sharp_subim_l[1] = 1+w_sharp;
blurry_subim_l[0] = 1;
blurry_subim_l[1] = 1+w_blurry;
/* Find number of processors available */
if(num_threads == 1) {
mexErrMsgTxt("num_threads must be greater than 1");
/* mexErrMsgIdAndTxt("deblur:numthreads","num_threads must be greater than 1. Your system reports having %d processors.",sysconf(_SC_NPROCESSORS_ONLN)); */
}
/* Number of threads must be an even number */
num_threads = min(ceil(num_threads/2), w_blurry)*2;
/* Invert calibration matrix for blurry image */
inv3(Kblurry,invKblurry);
int t;
mxArray *Ai_ptr[num_threads];
double *Ai[num_threads];
mxArray *w1w2_ptr[num_threads];
double *w1w2[num_threads];
mxArray *w2_ptr[num_threads];
double *w2[num_threads];
mxArray *f1f2_ptr[num_threads];
double *f1f2[num_threads];
mxArray *f2_ptr[num_threads];
double *f2[num_threads];
mxArray *mu1_ptr[num_threads];
double *mu1[num_threads];
mxArray *mu2_ptr[num_threads];
double *mu2[num_threads];
mxArray *exp_sqr_err_ptr[num_threads];
double *exp_sqr_err[num_threads];
double *Bi[num_threads];
int *xjfloor[num_threads];
int *yjfloor[num_threads];
double *coeffs[num_threads];
for(t=0; t<num_threads; ++t) {
/* general */
Ai_ptr[t] = mxCreateNumericArray(2, dims_sharp, mxDOUBLE_CLASS, mxREAL);
Ai[t] = mxGetPr(Ai_ptr[t]);
/* exp_sqr_err */
exp_sqr_err_ptr[t] = mxCreateNumericArray(2, dims_blurry, mxDOUBLE_CLASS, mxREAL);
exp_sqr_err[t] = mxGetPr(exp_sqr_err_ptr[t]);
/* f1f2 */
f1f2_ptr[t] = mxCreateNumericArray(2, dims_sharp, mxDOUBLE_CLASS, mxREAL);
f1f2[t] = mxGetPr(f1f2_ptr[t]);
/* f2 */
f2_ptr[t] = mxCreateNumericArray(2, dims_sharp, mxDOUBLE_CLASS, mxREAL);
f2[t] = mxGetPr(f2_ptr[t]);
/* w1w2 */
w1w2_ptr[t] = mxCreateDoubleMatrix(n_kernel, 1, mxREAL);
w1w2[t] = mxGetPr(w1w2_ptr[t]);
/* w2 */
w2_ptr[t] = mxCreateDoubleMatrix(n_kernel, 1, mxREAL);
w2[t] = mxGetPr(w2_ptr[t]);
/* mu1 = sum(sum(Dp.*(D-mD))); */
mu1_ptr[t] = mxCreateDoubleMatrix(1, 1, mxREAL);
mu1[t] = mxGetPr(mu1_ptr[t]);
/* mu2 = data_points */
mu2_ptr[t] = mxCreateDoubleMatrix(1, 1, mxREAL);
mu2[t] = mxGetPr(mu2_ptr[t]);
/* Temporary storage for threads */
Bi[t] = (double*) mxCalloc(n_kernel, sizeof(double));
xjfloor[t] = (int*) mxCalloc(n_kernel, sizeof(int));
yjfloor[t] = (int*) mxCalloc(n_kernel, sizeof(int));
coeffs[t] = (double*) mxCalloc(n_kernel*n_interp, sizeof(double));
}
mxArray* rerror_ptr = mxCreateDoubleMatrix(1, 1, mxREAL);
double *rerror = mxGetPr(rerror_ptr);
/* Hcache, used to cache homography matrices */
mxArray* Hcache_ptr = mxCreateDoubleMatrix(9, n_kernel, mxREAL);
Hcache = mxGetPr(Hcache_ptr);
int i, j, k;
/* Calculate variance of each kernel element */
mxArray *var_w_ptr = mxCreateDoubleMatrix(1, n_kernel, mxREAL);
var_w = mxGetPr(var_w_ptr);
for(k=0; k<n_kernel; ++k)
var_w[k] = mw2[k] - sqr(mw1[k]);
/* Calculate variance of each sharp pixel */
mxArray *var_f_ptr = mxCreateNumericArray(2, dims_sharp, mxDOUBLE_CLASS, mxREAL);
var_f = mxGetPr(var_f_ptr);
for(j=0; j<n_sharp; ++j)
var_f[j] = mf2[j] - sqr(mf1[j]);
/* Calculate homographies and cache them */
for(k=0; k<n_kernel; ++k)
compute_homography_matrix(Ksharp, &theta_list[k*3], invKblurry, &Hcache[k*9]);
/* Setup multiple threads */
pthread_t thread[num_threads];
struct thread_data thread_data_array[num_threads];
void *status;
int rc=0;
pthread_attr_t attr;
/* Initialize and set thread detached attribute so that we can join the thread */
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
/* Divide each subimage of blurry image into num_threads/2 slices */
int thread_width = (int)ceil(((double)w_blurry)/((double)num_threads/2));
/* Launch num_threads/2 threads per subimage */
for(t=0; t<num_threads; ++t) {
int subimage = (t < num_threads/2)? 0 : 1;
thread_data_array[t].thread_id = t;
thread_data_array[t].top = 1;
thread_data_array[t].bottom = h_blurry;
thread_data_array[t].left = t*thread_width+1-blurry_subim_l[subimage]+1;
thread_data_array[t].right = min((t+1)*thread_width-blurry_subim_l[subimage]+1,w_blurry);
thread_data_array[t].subimage = subimage;
/* Assign each thread a set of arrays for its outputs */
thread_data_array[t].Ai = Ai[t];
thread_data_array[t].w1w2 = w1w2[t];
thread_data_array[t].w2 = w2[t];
thread_data_array[t].f1f2 = f1f2[t];
thread_data_array[t].f2 = f2[t];
thread_data_array[t].mu1 = mu1[t];
thread_data_array[t].mu2 = mu2[t];
thread_data_array[t].exp_sqr_err = exp_sqr_err[t];
thread_data_array[t].Bi = Bi[t];
thread_data_array[t].xjfloor = xjfloor[t];
thread_data_array[t].yjfloor = yjfloor[t];
thread_data_array[t].coeffs = coeffs[t];
rc = pthread_create(&thread[t], &attr, thread_func, (void *) &thread_data_array[t]);
errorCheck("pthread_create()", rc);
}
/* Free attribute and wait for the other threads */
pthread_attr_destroy(&attr);
/* Join threads to wait until they're all finished */
for(t=0; t<num_threads; ++t) {
rc = pthread_join(thread[t], &status);
errorCheck("pthread_join()", rc);
if (status != NULL) {
mexPrintf("thread %ld exited with status %ld",t,(long)status);
mexErrMsgTxt("\n");
}
}
/* Combine threads' results */
/* Add threads' results together */
for(t=1; t<num_threads; ++t) {
for(j=0; j<n_sharp; ++j) {
(f1f2[0])[j] += (f1f2[t])[j];
(f2[0])[j] += (f2[t])[j];
}
for(k=0; k<n_kernel; ++k) {
(w1w2[0])[k] += (w1w2[t])[k];
(w2[0])[k] += (w2[t])[k];
}
*(mu1[0]) += *(mu1[t]);
*(mu2[0]) += *(mu2[t]);
}
/* Final addition, f1f2 += mf1.*f2 */
for(j=0; j<n_sharp; ++j) {
(f1f2[0])[j] += mf1[j]*(f2[0])[j];
}
for(k=0; k<n_kernel; ++k) {
(w1w2[0])[k] += mw1[k]*(w2[0])[k];
}
*rerror = 0;
for(t=0; t<num_threads; ++t) {
for(i=0; i<n_blurry; ++i) {
/* rerror is the sum of exp_sqr_err over all pixels */
*rerror += (exp_sqr_err[t])[i];
}
}
plhs[0] = f1f2_ptr[0];
plhs[1] = f2_ptr[0];
plhs[2] = w1w2_ptr[0];
plhs[3] = w2_ptr[0];
plhs[4] = mu1_ptr[0];
plhs[5] = mu2_ptr[0];
plhs[6] = rerror_ptr;
/* pthread_exit(NULL); */
}
matrix_2x2_t m2_=mmul2(m2, m2i);
print_util_print_matrix(print_util_get_debug_stream(), m2_.v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_3x3_t m3={.v={{1, 2, 4},{2,4,2},{4,2,1}}};
print_util_print_matrix(print_util_get_debug_stream(), m3.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_3x3_t m3d=diag_3x3(row3(m3, 0));
print_util_print_matrix(print_util_get_debug_stream(), m3d.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_3x3_t m3t=trans3(m3);
print_util_print_matrix(print_util_get_debug_stream(), m3t.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_3x3_t m3i=inv3(m3);
print_util_print_matrix(print_util_get_debug_stream(), m3i.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_3x3_t m3_=mmul3(m3, m3i);
m3_=mmul3(m3, inv3(m3));
print_util_print_matrix(print_util_get_debug_stream(), m3_.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_4x4_t m4={.v={{1, 2, 3, 4},{2,4,2, 4},{4,3,2,1}, {5,6,7,5}}};
print_util_print_matrix(print_util_get_debug_stream(), m4.v[0], 4, 4, 2); print_util_dbg_print("\n");
matrix_4x4_t m4t=trans4(m4);
print_util_print_matrix(print_util_get_debug_stream(), m4t.v[0], 4, 4, 2); print_util_dbg_print("\n");
matrix_4x4_t m4i=inv4(m4);
print_util_print_matrix(print_util_get_debug_stream(), m4i.v[0], 4, 4, 2); print_util_dbg_print("\n");