/* angle_acc() calculates the angle between the (1st order) numerical velocity
vectors to the predicted next position and to the candidate actual position.
The angle is calculated in [gon], see [1].
The predicted position is the position if the particle continued at current
velocity.
Arguments:
vec3d start, pred, cand - the particle start position, predicted position,
and possible actual position, respectively.
double *angle - output variable, the angle between the two velocity
vectors, [gon]
double *acc - output variable, the 1st-order numerical acceleration embodied
in the deviation from prediction.
*/
void angle_acc(vec3d start, vec3d pred, vec3d cand, double *angle, double *acc)
{
vec3d v0, v1;
vec_subt(pred, start, v0);
vec_subt(cand, start, v1);
*acc = vec_diff_norm(v0, v1);
if ((v0[0] == -v1[0]) && (v0[1] == -v1[1]) && (v0[2] == -v1[2])) {
*angle = 200;
} else if ((v0[0] == v1[0]) && (v0[1] == v1[1]) && (v0[2] == v1[2])) {
*angle = 0; // otherwise it returns NaN
} else {
*angle = (200./M_PI) * acos(vec_dot(v0, v1) / vec_norm(v0) \
/ vec_norm(v1));
}
}
void norm(double complex *a, int sz){
int i;
double nor;
nor = vec_norm(a, sz);
for (i=0; i<sz; i++){
a[i] /= nor;
}
}
stage get_stage(char *fut)
{
camera test_camera_1 = {{0,0,-5},288,512};
/* n.b. 16x9 is the cinematic widescreen ratio :-) */
sphere sph0 = {{0.67,0,4},0.67,{0.4,0.6,1.0},{1.0, 1.0, 1.0}};
sphere_list *spheres = sl_singleton(sph0);
vec ldir = vec_norm(vec_expr(-3,1,-1));
light lt = {ldir,{1,1,1}};
scene test_scene_1 = {{0.1,0.1,0.4},spheres,lt,{0.33,0.33,0.33}};
stage g = {test_camera_1, test_scene_1};
return g;
}
t_point3 vec_normalize(t_point3 vec)
{
t_point3 normalized;
double norm;
norm = vec_norm(vec);
if (norm == 0)
norm = 0.0001f;
normalized.x = vec.x / norm;
normalized.y = vec.y / norm;
normalized.z = vec.z / norm;
return (normalized);
}
t_hit plane_z(const t_ray *ray, const t_obj *obj)
{
t_hit res;
double k;
k = (ray->alpha).z / -(ray->beta).z;
res.hitpos.x = ((ray->beta).x * k + (ray->alpha).x);
res.hitpos.y = ((ray->beta).y * k + (ray->alpha).y);
res.hitpos.z = 0 ;
res.norm = vec_norm(&(ray->alpha), &(res.hitpos));
res.hit = (k > 0 ? 1 : 0);
res.obj = obj;
return (res);
}
frame compute_frame(timestamp t)
{
frame f;
f.ts = t;
f.bg = rgb_expr(.1, .1, .4);
fsphere sph0 = {vec_expr(.67, 0, 4), 0.67, compute_surf, compute_shine};
fsphere_list *spheres = fsl_cons(sph0, NULL);
f.spheres = spheres;
vec ldir = vec_norm(vec_expr(-3,1,-1));
light lt = {ldir,{1,1,1}};
f.light = lt;
rgb amb = rgb_expr(.33, .33, .33);
f.amb = amb;
return f;
}
void render(stage g)
{
int i, j;
camera c = g.c;
scene sc = g.s;
printf("P3\n");
printf("%d %d\n", c.w, c.h);
printf("255\n");
for(i=0; i < c.h; i++) {
for(j=0; j < c.w; j++) {
vec p = {j,i,0};
vec loc = logical_loc(c, p);
vec dir = vec_sub(loc, c.loc);
vec normdir = vec_norm(dir);
ray r = {c.loc, normdir};
rgb col = trace_ray(sc, r);
rgb_print_bytes(col);
}
}
}
hit_test intersect(ray r, sphere s)
{
hit_test result = {0}; /* {0} to quiet the compiler */
vec sc = s.center;
double sr = s.radius;
vec A = vec_sub(r.origin, sc);
double B = vec_dot(A, r.direction);
double C = vec_dot(A, A) - (sr * sr);
double D = (B * B) - C;
double t = (-B - sqrt(D));
result.miss = 1;
if (D > 0 && t > 0) {
result.miss = 0;
result.dist = t;
result.surf = s.surf;
result.shine = s.shine;
result.surf_norm = vec_norm(vec_sub(ray_position(r,t),sc));
}
return result;
}
/* weighted_dumbbell_precision() Gives a weighted sum of dumbbell precision
measures: ray convergence, and dumbbell length. The weight of the first is
1, and the later is weighted by a user defined value.
Arguments:
(vec2d **) targets - 2D array of targets, so order 3 tensor of shape
(num_targs,num_cams,2). Each target is the 2D metric, flat, centred
coordinates of one identified point.
int num_targs - the number of known targets, assumed to be the same in all
cameras.
int num_cams - number of cameras.
mm_np *multimed_pars - multimedia parameters struct for ray tracing through
several layers.
Calibration* cals[] - each camera's calibration object.
int db_length - distance between two consecutive targets (assuming the
targets list is a 2 by 2 list of dumbbell frames).
double db_weight - the weight of the average length error compared to
the average ray convergence error.
*/
double weighted_dumbbell_precision(vec2d** targets, int num_targs, int num_cams,
mm_np *multimed_pars, Calibration* cals[], int db_length, double db_weight)
{
int pt;
double dtot = 0, len_err_tot = 0, dist;
vec3d res[2], *res_current;
for (pt = 0; pt < num_targs; pt++) {
res_current = &(res[pt % 2]);
dtot += point_position(targets[pt], num_cams, multimed_pars, cals,
*res_current);
if (pt % 2 == 1) {
vec_subt(res[0], res[1], res[0]);
dist = vec_norm(res[0]);
len_err_tot += 1 - ((dist > db_length) ? (db_length/dist) : dist/db_length);
}
}
/* note half as many pairs as targets is assumed */
return (dtot / num_targs + db_weight*len_err_tot/(0.5*num_targs));
}
void palImpulseActuator::Apply()
{
palMatrix4x4 m;
mat_identity(&m);
mat_translate(&m,m_fRelativePosX,m_fRelativePosY,m_fRelativePosZ);
//printf("rel:%f %f %f ",m._41,m._42,m._43);
palMatrix4x4 bodypos = m_pBody->GetLocationMatrix();
palMatrix4x4 out;
mat_multiply(&out,&bodypos,&m);
palVector3 newpos;
newpos.x=out._41;
newpos.y=out._42;
newpos.z=out._43;
// printf("output : %f %f %f ",out._41,out._42,out._43);
// imp_pos=out;
mat_identity(&m);
mat_translate(&m,m_fAxisX,m_fAxisY,m_fAxisZ);
mat_multiply(&out,&bodypos,&m);
// printf("output : %f %f %f\n",out._41,out._42,out._43);
// imp_axis=out;
palVector3 newaxis;
newaxis.x=out._41-bodypos._41;
newaxis.y=out._42-bodypos._42;
newaxis.z=out._43-bodypos._43;
vec_norm(&newaxis);
m_pBody->ApplyImpulseAtPosition(newpos.x,newpos.y,newpos.z,newaxis.x*m_fImpulse,newaxis.y*m_fImpulse,newaxis.z*m_fImpulse);
}
int dir_of_vec(struct vec *v) {
double x = v->x;
double y = v->y;
const double z = 0.9239;
const double mz = -0.9239;
double len = vec_norm(v);
int d = urandom(8);
if (len > 0.0001) {
x = x/len;
y = y/len;
{
if (x > z)
d = 0; // E
else if (x < mz)
d = 4; // W
else if (y > z)
d = 2; // N
else if (y < mz)
d = 6; // S
else if (x > 0.0) {
if (y > 0.0)
d = 1; // NE
else
d = 7; // SE
}
else {
if (y > 0.0)
d = 3; // NW
else
d = 5; // SW
}
}
}
return d;
}
int vec_unit(double a[]) {
double norm = vec_norm(a);
vec_scap(1/norm,a);
}
/* orient() calculates orientation of the camera, updating its calibration
structure using the definitions and algorithms well described in [1].
Arguments:
Calibration* cal_in - camera calibration object
control_par *cpar - control parameters
int nfix - number of 3D known points
vec3d fix[] - each of nfix items is one 3D position of known point on
the calibration object.
target pix[] - image coordinates corresponding to each point in ``fix``.
can be obtained from the set of detected 2D points using
sortgrid(). The points which are associated with fix[] have real
pointer (.pnr attribute), others have -999.
orient_par flags - structure of all the flags of the parameters to be
(un)changed, read from orient.par parameter file using
read_orient_par(), defaults are zeros except for x_scale which is
by default 1.
Output:
Calibration *cal_in - if the orientation routine converged, this structure
is updated, otherwise, returned untouched. The routine works on a copy of
the calibration structure, cal.
double sigmabeta[] - array of deviations for each of the interior and
exterior parameters and glass interface vector (19 in total).
Returns:
On success, a pointer to an array of residuals. For each observation point
i = 0..n-1, residual 2*i is the Gauss-Markof residual for the x coordinate
and residual 2*i + 1 is for the y. Then come 10 cells with the delta
between initial guess and final solution for internal and distortion
parameters, which are also part of the G-M model and described in it.
On failure returns NULL.
*/
double* orient (Calibration* cal_in, control_par *cpar, int nfix, vec3d fix[],
target pix[], orient_par *flags, double sigmabeta[20])
{
int i,j,n, itnum, stopflag, n_obs=0, maxsize;
double ident[IDT], XPX[NPAR][NPAR], XPy[NPAR], beta[NPAR], omega=0;
double xp, yp, xpd, ypd, xc, yc, r, qq, p, sumP;
int numbers;
double al,be,ga,nGl,e1_x,e1_y,e1_z,e2_x,e2_y,e2_z,safety_x,safety_y,safety_z;
double *P, *y, *yh, *Xbeta, *resi;
vec3d glass_dir, tmp_vec, e1, e2;
Calibration *cal;
/* small perturbation for translation/rotation in meters and in radians */
double dm = 0.00001, drad = 0.0000001;
cal = malloc (sizeof (Calibration));
memcpy(cal, cal_in, sizeof (Calibration));
maxsize = nfix*2 + IDT;
P = (double *) calloc(maxsize, sizeof(double));
y = (double *) calloc(maxsize, sizeof(double));
yh = (double *) calloc(maxsize, sizeof(double));
Xbeta = (double *) calloc(maxsize, sizeof(double));
resi = (double *) calloc(maxsize, sizeof(double));
double (*X)[NPAR] = malloc(sizeof (*X) * maxsize);
double (*Xh)[NPAR] = malloc(sizeof (*Xh) * maxsize);
for(i = 0; i < maxsize; i++) {
for(j = 0; j < NPAR; j++) {
X[i][j] = 0.0;
Xh[i][j] = 0.0;
}
y[i] = 0;
P[i] = 1;
}
for(i = 0; i < NPAR; i++)
sigmabeta[j] = 0.0;
if(flags->interfflag){
numbers = 18;
} else{
numbers = 16;
}
vec_set(glass_dir,
cal->glass_par.vec_x, cal->glass_par.vec_y, cal->glass_par.vec_z);
nGl = vec_norm(glass_dir);
e1_x = 2*cal->glass_par.vec_z - 3*cal->glass_par.vec_x;
e1_y = 3*cal->glass_par.vec_x - 1*cal->glass_par.vec_z;
e1_z = 1*cal->glass_par.vec_y - 2*cal->glass_par.vec_y;
vec_set(tmp_vec, e1_x, e1_y, e1_z);
unit_vector(tmp_vec, e1);
e2_x = e1_y*cal->glass_par.vec_z - e1_z*cal->glass_par.vec_x;
e2_y = e1_z*cal->glass_par.vec_x - e1_x*cal->glass_par.vec_z;
e2_z = e1_x*cal->glass_par.vec_y - e1_y*cal->glass_par.vec_y;
vec_set(tmp_vec, e2_x, e2_y, e2_z);
unit_vector(tmp_vec, e2);
al = 0;
be = 0;
ga = 0;
/* init identities */
ident[0] = cal->int_par.cc;
ident[1] = cal->int_par.xh;
ident[2] = cal->int_par.yh;
ident[3] = cal->added_par.k1;
ident[4] = cal->added_par.k2;
ident[5] = cal->added_par.k3;
ident[6] = cal->added_par.p1;
ident[7] = cal->added_par.p2;
ident[8] = cal->added_par.scx;
ident[9] = cal->added_par.she;
safety_x = cal->glass_par.vec_x;
safety_y = cal->glass_par.vec_y;
safety_z = cal->glass_par.vec_z;
/* main loop, program runs through it, until none of the beta values
comes over a threshold and no more points are thrown out
because of their residuals */
itnum = 0;
stopflag = 0;
while ((stopflag == 0) && (itnum < NUM_ITER)) {
itnum++;
for (i = 0, n = 0; i < nfix; i++) {
/* check for correct correspondence
note that we do not use anymore pointer in fix, the points are read by
the order of appearance and if we want to use every other point
we use 'i', just check it is not -999 */
if(pix[i].pnr != i) continue;
switch (flags->useflag) {
case 1: if ((i % 2) == 0) continue; break;
case 2: if ((i % 2) != 0) continue; break;
case 3: if ((i % 3) == 0) continue; break;
}
/* get metric flat-image coordinates of the detected point */
pixel_to_metric (&xc, &yc, pix[i].x, pix[i].y, cpar);
correct_brown_affin (xc, yc, cal->added_par, &xc, &yc);
/* Projected 2D position on sensor of corresponding known point */
rotation_matrix(&(cal->ext_par));
img_coord (fix[i], cal, cpar->mm, &xp, &yp);
/* derivatives of distortion parameters */
r = sqrt (xp*xp + yp*yp);
X[n][7] = cal->added_par.scx;
X[n+1][7] = sin(cal->added_par.she);
X[n][8] = 0;
X[n+1][8] = 1;
X[n][9] = cal->added_par.scx * xp * r*r;
X[n+1][9] = yp * r*r;
X[n][10] = cal->added_par.scx * xp * pow(r,4.0);
X[n+1][10] = yp * pow(r,4.0);
X[n][11] = cal->added_par.scx * xp * pow(r,6.0);
X[n+1][11] = yp * pow(r,6.0);
X[n][12] = cal->added_par.scx * (2*xp*xp + r*r);
X[n+1][12] = 2 * xp * yp;
X[n][13] = 2 * cal->added_par.scx * xp * yp;
X[n+1][13] = 2*yp*yp + r*r;
qq = cal->added_par.k1*r*r; qq += cal->added_par.k2*pow(r,4.0);
qq += cal->added_par.k3*pow(r,6.0);
qq += 1;
X[n][14] = xp * qq + cal->added_par.p1 * (r*r + 2*xp*xp) + \
2*cal->added_par.p2*xp*yp;
X[n+1][14] = 0;
X[n][15] = -cos(cal->added_par.she) * yp;
X[n+1][15] = -sin(cal->added_par.she) * yp;
/* numeric derivatives of projection coordinates over external
parameters, 3D position and the angles */
num_deriv_exterior(cal, cpar, dm, drad, fix[i], X[n], X[n + 1]);
/* Num. deriv. of projection coords over sensor distance from PP */
cal->int_par.cc += dm;
rotation_matrix(&(cal->ext_par));
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][6] = (xpd - xp) / dm;
X[n+1][6] = (ypd - yp) / dm;
cal->int_par.cc -= dm;
/* ditto, over water-glass-air interface position vector */
al += dm;
cal->glass_par.vec_x += e1[0]*nGl*al;
cal->glass_par.vec_y += e1[1]*nGl*al;
cal->glass_par.vec_z += e1[2]*nGl*al;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][16] = (xpd - xp) / dm;
X[n+1][16] = (ypd - yp) / dm;
al -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
be += dm;
cal->glass_par.vec_x += e2[0]*nGl*be;
cal->glass_par.vec_y += e2[1]*nGl*be;
cal->glass_par.vec_z += e2[2]*nGl*be;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][17] = (xpd - xp) / dm;
X[n+1][17] = (ypd - yp) / dm;
be -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
ga += dm;
cal->glass_par.vec_x += cal->glass_par.vec_x*nGl*ga;
cal->glass_par.vec_y += cal->glass_par.vec_y*nGl*ga;
cal->glass_par.vec_z += cal->glass_par.vec_z*nGl*ga;
img_coord (fix[i], cal, cpar->mm, &xpd, &ypd);
X[n][18] = (xpd - xp) / dm;
X[n+1][18] = (ypd - yp) / dm;
ga -= dm;
cal->glass_par.vec_x = safety_x;
cal->glass_par.vec_y = safety_y;
cal->glass_par.vec_z = safety_z;
y[n] = xc - xp;
y[n+1] = yc - yp;
n += 2;
}
n_obs = n;
/* identities */
for (i = 0; i < IDT; i++)
X[n_obs + i][6 + i] = 1;
y[n_obs+0] = ident[0] - cal->int_par.cc;
y[n_obs+1] = ident[1] - cal->int_par.xh;
y[n_obs+2] = ident[2] - cal->int_par.yh;
y[n_obs+3] = ident[3] - cal->added_par.k1;
y[n_obs+4] = ident[4] - cal->added_par.k2;
y[n_obs+5] = ident[5] - cal->added_par.k3;
y[n_obs+6] = ident[6] - cal->added_par.p1;
y[n_obs+7] = ident[7] - cal->added_par.p2;
y[n_obs+8] = ident[8] - cal->added_par.scx;
y[n_obs+9] = ident[9] - cal->added_par.she;
/* weights */
for (i = 0; i < n_obs; i++)
P[i] = 1;
P[n_obs+0] = ( ! flags->ccflag) ? POS_INF : 1;
P[n_obs+1] = ( ! flags->xhflag) ? POS_INF : 1;
P[n_obs+2] = ( ! flags->yhflag) ? POS_INF : 1;
P[n_obs+3] = ( ! flags->k1flag) ? POS_INF : 1;
P[n_obs+4] = ( ! flags->k2flag) ? POS_INF : 1;
P[n_obs+5] = ( ! flags->k3flag) ? POS_INF : 1;
P[n_obs+6] = ( ! flags->p1flag) ? POS_INF : 1;
P[n_obs+7] = ( ! flags->p2flag) ? POS_INF : 1;
P[n_obs+8] = ( ! flags->scxflag) ? POS_INF : 1;
P[n_obs+9] = ( ! flags->sheflag) ? POS_INF : 1;
n_obs += IDT;
sumP = 0;
for (i = 0; i < n_obs; i++) { /* homogenize */
p = sqrt (P[i]);
for (j = 0; j < NPAR; j++)
Xh[i][j] = p * X[i][j];
yh[i] = p * y[i];
sumP += P[i];
}
/* Gauss Markoff Model it is the least square adjustment
of the redundant information contained both in the spatial
intersection and the resection, see [1], eq. 23 */
ata ((double *) Xh, (double *) XPX, n_obs, numbers, NPAR );
matinv ((double *) XPX, numbers, NPAR);
atl ((double *) XPy, (double *) Xh, yh, n_obs, numbers, NPAR);
matmul ((double *) beta, (double *) XPX, (double *) XPy,
numbers, numbers,1, NPAR, NPAR);
stopflag = 1;
for (i = 0; i < numbers; i++) {
if (fabs (beta[i]) > CONVERGENCE) stopflag = 0;
}
if ( ! flags->ccflag) beta[6] = 0.0;
if ( ! flags->xhflag) beta[7] = 0.0;
if ( ! flags->yhflag) beta[8] = 0.0;
if ( ! flags->k1flag) beta[9] = 0.0;
if ( ! flags->k2flag) beta[10] = 0.0;
if ( ! flags->k3flag) beta[11] = 0.0;
if ( ! flags->p1flag) beta[12] = 0.0;
if ( ! flags->p2flag) beta[13] = 0.0;
if ( ! flags->scxflag)beta[14] = 0.0;
if ( ! flags->sheflag) beta[15] = 0.0;
cal->ext_par.x0 += beta[0];
cal->ext_par.y0 += beta[1];
cal->ext_par.z0 += beta[2];
cal->ext_par.omega += beta[3];
cal->ext_par.phi += beta[4];
cal->ext_par.kappa += beta[5];
cal->int_par.cc += beta[6];
cal->int_par.xh += beta[7];
cal->int_par.yh += beta[8];
cal->added_par.k1 += beta[9];
cal->added_par.k2 += beta[10];
cal->added_par.k3 += beta[11];
cal->added_par.p1 += beta[12];
cal->added_par.p2 += beta[13];
cal->added_par.scx += beta[14];
cal->added_par.she += beta[15];
if (flags->interfflag) {
cal->glass_par.vec_x += e1[0]*nGl*beta[16];
cal->glass_par.vec_y += e1[1]*nGl*beta[16];
cal->glass_par.vec_z += e1[2]*nGl*beta[16];
cal->glass_par.vec_x += e2[0]*nGl*beta[17];
cal->glass_par.vec_y += e2[1]*nGl*beta[17];
cal->glass_par.vec_z += e2[2]*nGl*beta[17];
}
}
/* compute residuals etc. */
matmul ( (double *) Xbeta, (double *) X, (double *) beta, n_obs,
numbers, 1, n_obs, NPAR);
omega = 0;
for (i = 0; i < n_obs; i++) {
resi[i] = Xbeta[i] - y[i];
omega += resi[i] * P[i] * resi[i];
}
sigmabeta[NPAR] = sqrt (omega / (n_obs - numbers));
for (i = 0; i < numbers; i++) {
sigmabeta[i] = sigmabeta[NPAR] * sqrt(XPX[i][i]);
}
free(X);
free(P);
free(y);
free(Xbeta);
free(Xh);
if (stopflag){
rotation_matrix(&(cal->ext_par));
memcpy(cal_in, cal, sizeof (Calibration));
return resi;
}
else {
free(resi);
return NULL;
}
}
void WeightMatrix_iter::weight_matrix(std::vector< std::vector<double> >& allfeatures,
bool exhaustive)
{
std::vector< std::vector<double> > pfeatures;
copy(allfeatures, pfeatures);
_nrows = pfeatures.size();
_ncols = pfeatures[0].size();
EstimateBandwidth(pfeatures, _deltas);
_wtmat.resize(_nrows);
_degree.resize(_nrows, 0.0);
unsigned long nnz=0;
std::map<double, unsigned int> sorted_features;
for(size_t i=0; i < pfeatures.size(); i++){
std::vector<double>& tmpvec = pfeatures[i];
scale_vec(tmpvec, _deltas);
double nrm = vec_norm(tmpvec);
sorted_features.insert(std::make_pair(nrm, i));
}
std::map<double, unsigned int>::iterator sit_i;
std::map<double, unsigned int>::iterator sit_j;
for(sit_i = sorted_features.begin() ; sit_i != sorted_features.end(); sit_i++){
sit_j = sit_i;
sit_j++;
size_t i = sit_i->second;
unsigned int nchecks = 0;
for(; sit_j != sorted_features.end(); sit_j++){
size_t j = sit_j->second;
double dot_ij = vec_dot(pfeatures[i], pfeatures[j]);
double dist = sit_i->first + sit_j->first - 2*dot_ij;
double min_dist = sit_i->first + sit_j->first - 2*sqrt(sit_i->first)*sqrt(sit_j->first);
nchecks++;
if (dist < (_thd*_thd)){
double val = exp(-dist/(0.5*_thd*_thd));
_wtmat[i].push_back(RowVal(j, val));
(_degree[i]) += val;
_wtmat[j].push_back(RowVal(i, val));
(_degree[j]) += val;
nnz += 2;
}
if (min_dist>(_thd*_thd)){
break;
}
}
}
printf("total nonzeros: %lu\n",nnz);
/* C debug*
FILE* fp=fopen("semi-supervised/tmp_wtmatrix.txt", "wt");
for(size_t rr = 0; rr < _wtmat.size(); rr++){
double luu = 0;
double w_l = 0;
for(size_t cc=0; cc < _wtmat[rr].size(); cc++){
size_t cidx = _wtmat[rr][cc].j;
double val = _wtmat[rr][cc].v;
//val = exp(-val/(0.5*_thd*_thd));
fprintf(fp, "%u %u %lf\n", rr, cidx, val);
}
}
fclose(fp);
//*C*/
}
void game_player_tick(player_t *pl, float dt)
{
if(pl->magic != 0xC4 && pl->magic != 0xC9 && pl->magic != 0x66 && pl->magic != 0x69)
return;
camera_t *cam = &(pl->cam);
float vs = 0.12f*100.0f*dt;
// trace motion
v4f_t no, tno, tv;
no.m = _mm_setzero_ps();
if(pl->magic != 0x66 && pl->magic != 0x69)
{
if(pl->vflip)
{
no.m = _mm_add_ps(no.m, _mm_mul_ps(cam->m.v.x.m, _mm_set1_ps(-pl->lv.v.x*vs)));
no.m = _mm_add_ps(no.m, _mm_mul_ps(cam->m.v.y.m, _mm_set1_ps(pl->lv.v.y*vs)));
no.m = _mm_add_ps(no.m, _mm_mul_ps(cam->m.v.z.m, _mm_set1_ps(pl->lv.v.w*vs)));
no.m = _mm_add_ps(no.m, _mm_mul_ps(cam->m.v.w.m, _mm_set1_ps(pl->lv.v.z*vs)));
} else {
no.m = _mm_add_ps(no.m, _mm_mul_ps(cam->m.v.x.m, _mm_set1_ps(pl->lv.v.x*vs)));
no.m = _mm_add_ps(no.m, _mm_mul_ps(cam->m.v.y.m, _mm_set1_ps(pl->lv.v.y*vs)));
no.m = _mm_add_ps(no.m, _mm_mul_ps(cam->m.v.z.m, _mm_set1_ps(pl->lv.v.z*vs)));
no.m = _mm_add_ps(no.m, _mm_mul_ps(cam->m.v.w.m, _mm_set1_ps(pl->lv.v.w*vs)));
}
}
// add gravity
pl->grav_v += 0.3f*vs;
if(pl->grav_v > 2.5f)
pl->grav_v = 2.5f;
no.v.y = pl->grav_v*vs;
// check distance
v4f_t tv2;
tv2.m = _mm_mul_ps(no.m, no.m);
//float md = sqrtf(vx*vx + vy*vy + vz*vz + vw*vw)*vs;
float md = tv2.v.x + tv2.v.y + tv2.v.z + tv2.v.w;
if(md > 0.0000001f)
{
// normalise for direction
tv.m = no.m;
vec_norm(&tv);
// cast a ray
float r = 0.5f;
tno.m = cam->o.m;
int side;
// trace away
for(;;)
{
float d = trace_box(root, &tno, &tv, NULL, NULL, NULL, md + r, &side);
//
// apply collision
//
if(d < 0.0f)
{
// no collision. jump to point.
cam->o.m = _mm_add_ps(cam->o.m,
_mm_mul_ps(_mm_set1_ps(md), tv.m));
break;
} else {
// we've hit a plane. slide back.
if(side == F_YP)
{
if(pl->grav_v >= 0.0f)
{
pl->grav_v = 0.0f;
pl->grounded = 1;
}
} else if(side == F_YN) {
if(pl->grav_v <= 0.0f)
pl->grav_v = 0.0f;
}
float dd = md - (d - r);
cam->o.m = _mm_add_ps(cam->o.m,
_mm_mul_ps(_mm_set1_ps(md - dd), tv.m));
// mask out velocity.
tv.a[side&3] = 0.0f;
// reduce distance.
md = dd;
}
}
}
}
Float palRevoluteLink::GetAngle() const {
if (m_pParent==NULL) return 0.0f;
if (m_pChild ==NULL) return 0.0f;
//palMatrix4x4 a_PAL,b_PAL;
palMatrix4x4 a,b;
a=m_pParent->GetLocationMatrix();
b=m_pChild->GetLocationMatrix();
palVector3 fac0;
mat_get_row(&m_frameA,&fac0,0);
palVector3 refAxis0;
vec_mat_mul(&refAxis0,&a,&fac0);
palVector3 fac1;
mat_get_row(&m_frameA,&fac1,1);
palVector3 refAxis1;
vec_mat_mul(&refAxis1,&a,&fac1);
palVector3 fbc1;
mat_get_row(&m_frameB,&fbc1,1);
palVector3 swingAxis;
vec_mat_mul(&swingAxis,&b,&fbc1);
Float d0 = vec_dot(&swingAxis,&refAxis0);
Float d1 = vec_dot(&swingAxis,&refAxis1);
return std::atan2(d0,d1);
#if 0 //this method does not do +/-, just positive :(
palVector3 pp,cp;
m_pParent->GetPosition(pp);
m_pChild->GetPosition(cp);
// printf("pp:");
// printvector(&pp);
// printf("cp:");
// printvector(&cp);
palVector3 linkpos;
linkpos.x=m_fRelativePosX;
linkpos.y=m_fRelativePosY;
linkpos.z=m_fRelativePosZ;
// printf("lp:");
// printvector(&linkpos);
palVector3 newlp;
vec_mat_mul(&newlp,&a,&linkpos);
vec_add(&newlp,&newlp,&pp);
// printf("nlp:");
// printvector(&newlp);
palVector3 la,lb;
vec_sub(&la,&pp,&newlp);
vec_sub(&lb,&cp,&newlp);
// la = pp;
// lb = cp;
vec_norm(&la);
vec_norm(&lb);
// printvector(&la);
// printvector(&lb);
Float dot=vec_dot(&la,&lb);
Float mag=vec_mag(&la)*vec_mag(&lb);
return Float(acos(dot/mag));
#endif
}
void palRevoluteLink::Init(palBodyBase *parent, palBodyBase *child, Float x, Float y, Float z, Float axis_x, Float axis_y, Float axis_z) {
palLink::Init(parent, child, x, y, z);
if (m_pParent && m_pChild) {
//Link_rel with the rotation matrix
//Link_rel=(link_abs - parent_abs)*R
palMatrix4x4 a_PAL = m_pParent->GetLocationMatrix();
palMatrix4x4 b_PAL = m_pChild->GetLocationMatrix();
// Transpose each body's position matrix to get its world-to-body
// rotation matrix:
palMatrix4x4 a, b;
mat_transpose(&a, &a_PAL); // a <- a_PAL'
mat_transpose(&b, &b_PAL); // b <- b_PAL'
palVector3 link_rel;
palVector3 translation;
// Compute the position of the link with respect to the parent's
// origin, in the parent's coordinate system:
palVector3 posVecParent;
m_pParent->GetPosition(posVecParent);
translation._vec[0] = m_fPosX - posVecParent.x;
translation._vec[1] = m_fPosY - posVecParent.y;
translation._vec[2] = m_fPosZ - posVecParent.z;
//Rotation
vec_mat_mul(&link_rel,&a,&translation);
m_fRelativePosX = link_rel.x;
m_fRelativePosY = link_rel.y;
m_fRelativePosZ = link_rel.z;
m_pivotA.x = m_fRelativePosX;
m_pivotA.y = m_fRelativePosY;
m_pivotA.z = m_fRelativePosZ;
// Compute the position of the link with respect to the child's
// origin, in the child's coordinate system:
palVector3 posVecChild;
m_pChild->GetPosition(posVecChild);
//link relative position with respect to the child
//Translation of absolute to relative
translation._vec[0] = m_fPosX - posVecChild.x; // first in world coords
translation._vec[1] = m_fPosY - posVecChild.y;
translation._vec[2] = m_fPosZ - posVecChild.z;
vec_mat_mul(&link_rel,&b,&translation); // rotate into child coords
m_pivotB.x = link_rel.x;
m_pivotB.y = link_rel.y;
m_pivotB.z = link_rel.z;
//Frames A and B: Bullet method
// Define a hinge coordinate system by generating hinge-to-body
// transforms for both parent and child bodies. The hinge
// coordinate system's +Z axis coincides with the hinge axis, its
// +X and +Y axes are perpendicular to the hinge axis, and its
// origin is at the hinge's origin.
palVector3 axis, m_axisA, m_axisB;
vec_set(&axis, axis_x, axis_y, axis_z);
vec_mat_mul(&m_axisA, &a, &axis); // axis in parent coords
m_fRelativeAxisX = m_axisA.x;
m_fRelativeAxisY = m_axisA.y;
m_fRelativeAxisZ = m_axisA.z;
vec_mat_mul(&m_axisB, &b, &axis); // axis in child coords
vec_norm(&m_axisB);
// Build m_frameA, which transforms points from hinge coordinates
// to parent (body A) coordinates.
// Choose basis vectors for the hinge coordinate system wrt parent coords:
palVector3 rbAxisA1( 1, 0, 0 ), rbAxisA2;
const palVector3 Z_AXIS( 0, 0, 1 );
// XXX debug
Float projection = vec_dot( & m_axisA, & Z_AXIS );
if (projection >= 1 - FLOAT_EPSILON) {
// The hinge axis coincides with the parent's +Z axis.
rbAxisA2 = palVector3( 0, 1, 0 );
} else if (projection <= -1 + FLOAT_EPSILON) {
// The hinge axis coincides with the parent's -Z axis.
rbAxisA2 = palVector3( 0, -1, 0 );
} else {
// The hinge axis crosses the parent's Z axis.
vec_cross( & rbAxisA2, & m_axisA, & Z_AXIS );
vec_cross( & rbAxisA1, & rbAxisA2, & m_axisA );
vec_norm( & rbAxisA1 );
vec_norm( & rbAxisA2 );
}
vec_norm( & m_axisA );
// Set frameA. Transform m_frameA maps points from hinge coordinate system,
// whose +Z axis coincides with the hinge axis, to the parent body's
// coordinate system.
mat_identity(&m_frameA);
mat_set_translation(&m_frameA,m_pivotA.x,m_pivotA.y,m_pivotA.z);
// Put the basis vectors in the columns of m_frameA:
m_frameA._11 = rbAxisA1.x;
m_frameA._12 = rbAxisA1.y;
m_frameA._13 = rbAxisA1.z;
m_frameA._21 = rbAxisA2.x;
m_frameA._22 = rbAxisA2.y;
m_frameA._23 = rbAxisA2.z;
m_frameA._31 = m_axisA.x;
m_frameA._32 = m_axisA.y;
m_frameA._33 = m_axisA.z;
palVector3 rbAxisB1, rbAxisB2;
if (true) {
//build frame B, see bullet for algo
palQuaternion rArc;
q_shortestArc(&rArc,&m_axisA,&m_axisB);
vec_q_rotate(&rbAxisB1,&rArc,&rbAxisA1);
vec_cross(&rbAxisB2,&m_axisB,&rbAxisB1);
} else {
palVector3 tmp;
vec_mat_mul( &tmp, &a_PAL, &rbAxisA1 );
vec_mat_mul( &rbAxisB1, &b, &tmp );
vec_mat_mul( &tmp, &a_PAL, &rbAxisA2 );
vec_mat_mul( &rbAxisB2, &b, &tmp );
}
// Build m_frameB, which transforms points from hinge coordinates
// to parent (body A) coordinates.
vec_norm(&rbAxisB1);
vec_norm(&rbAxisB2);
mat_identity(&m_frameB);
mat_set_translation(&m_frameB,m_pivotB.x,m_pivotB.y,m_pivotB.z);
// Put the basis vectors in the columns of m_frameB:
m_frameB._11 = rbAxisB1.x;
m_frameB._12 = rbAxisB1.y;
m_frameB._13 = rbAxisB1.z;
m_frameB._21 = rbAxisB2.x;
m_frameB._22 = rbAxisB2.y;
m_frameB._23 = rbAxisB2.z;
m_frameB._31 = m_axisB.x;
m_frameB._32 = m_axisB.y;
m_frameB._33 = m_axisB.z;
}
}
void WeightMatrix_iter::weight_matrix_parallel(std::vector< std::vector<double> >& allfeatures,
bool exhaustive)
{
printf("running parallel version\n");
std::vector< std::vector<double> > pfeatures;
copy(allfeatures, pfeatures);
_nrows = pfeatures.size();
_ncols = pfeatures[0].size();
EstimateBandwidth(pfeatures, _deltas);
_wtmat.resize(_nrows);
_degree.resize(_nrows, 0.0);
size_t NCORES = 8;
std::map<double, unsigned int> sorted_features; // all features are unique, check learn or iterative learn algo
std::vector< std::map<double, unsigned int> > sorted_features_p(NCORES); // all features are unique, check learn or iterative learn algo
int part = 0;
for(size_t i=0; i < pfeatures.size(); i++){
std::vector<double>& tmpvec = pfeatures[i];
scale_vec(tmpvec, _deltas);
double nrm = vec_norm(tmpvec);
sorted_features.insert(std::make_pair(nrm, i));
(sorted_features_p[part]).insert(std::make_pair(nrm, i));
part = (part+1) % NCORES;
}
std::vector< boost::thread* > threads;
std::vector< std::vector< std::vector<RowVal> > > tmpmat(sorted_features_p.size());
std::vector< std::vector<double> > degrees(sorted_features_p.size());
for(size_t pp=0; pp < sorted_features_p.size(); pp++){
tmpmat[pp].resize(_nrows);
degrees[pp].resize(_nrows, 0.0);
//compute_weight_partial(sorted_features_p[i], sorted_features, pfeatures);
threads.push_back(new boost::thread(&WeightMatrix_iter::compute_weight_partial, this, sorted_features_p[pp], sorted_features, pfeatures, boost::ref(tmpmat[pp]), boost::ref(degrees[pp])));
}
printf("Sync all threads \n");
for (size_t ti=0; ti<threads.size(); ti++)
(threads[ti])->join();
printf("all threads done\n");
for(size_t pp=0; pp < tmpmat.size(); pp++){
for(size_t i=0; i < tmpmat[pp].size(); i++){
if(tmpmat[pp][i].size()>0){
_wtmat[i].insert(_wtmat[i].end(),tmpmat[pp][i].begin(), tmpmat[pp][i].end());
(_degree[i]) += degrees[pp][i];
}
}
}
if(_nrows != _wtmat.size()){
printf(" number of rows and wtmat size mismatch\n");
}
_nzd_indicator.resize(_nrows, 0);
_trn_indicator.resize(_nrows, 0);
for (size_t i=0; i < _nrows; i++){
if (_degree[i] > 0){
_nzd_indicator[i] = 1;
_nzd_map.insert(std::make_pair(i, _nzd_count) );
_nzd_invmap.insert(std::make_pair(_nzd_count, i) );
_nzd_count++;
}
}
compute_Wnorm();
// //** _nzd_count = non-zero degree count == number of columns
// //** _nnz = number of nonzeros
//
// _nnz = _Wnorm_i.size();
// cholmod_solver.initialize(_nzd_count, _nnz, _Wnorm_i.data(), _Wnorm_j.data(), _Wnorm_v.data());
/* C debug*
FILE* fp=fopen("semi-supervised/tmp_wtmatrix_parallel.txt", "wt");
for(size_t rr = 0; rr < _wtmat.size(); rr++){
double luu = 0;
double w_l = 0;
for(size_t cc=0; cc < _wtmat[rr].size(); cc++){
size_t cidx = _wtmat[rr][cc].j;
double val = _wtmat[rr][cc].v;
//val = exp(-val/(0.5*_thd*_thd));
fprintf(fp, "%u %u %lf\n", rr, cidx, val);
}
}
fclose(fp);
//*C*/
}
int vec_unit_wc(double out[], double a[]) {
double norm = vec_norm(a);
vec_scap_wc(out,1/norm,a);
}
/* Computes the color of a ray intersection */
vec_t ga_ray_shade( const vec_t * pos,
const vec_t * dir,
const vec_t * norm,
const ga_material_t *mat,
const ga_scene_t *s,
float importance,
int max_rec){
ga_node_t *n = NULL;
ga_light_t *light = NULL;
ga_material_t *comb = NULL;
vec_t color = vec_new(0,0,0,1);
vec_t dpos = vec_add(*pos,vec_scale(0.00001,*norm));
vec_t lsample,d1,d2,lcolor,ldir;
int sample;
float len;
float rlen;
/*dir = vec_norm(dir);*/
if(!mat || !pos || !dir ||!norm || !s){
return color;
}
/* global shading */
if(mat->type == GA_MATERIAL_BLENDING && mat->child){
return vec_scale(mat->blend_factor,
ga_ray_shade(pos,dir,norm,mat->child,s,importance*mat->blend_factor, max_rec));
}
if(mat->type == GA_MATERIAL_COMB && mat->comb){
n = mat->comb->first;
while(n){
comb = (ga_material_t*)n->data;
color = vec_add(color, ga_ray_shade(pos,dir,norm,comb,s,importance,max_rec));
n = n->next;
}
return color;
}
if(mat->ref_factor != 0.0f){
ga_shade_reflect(&color,s,mat,&dpos,dir,norm,importance,max_rec);
}
if(mat->emit_factor != 0.0f){
ga_shade_emit(&color,mat);
}
if(mat->ao_factor != 0.0f && mat->ao_sample){
ga_shade_ao(&color,s,mat,&dpos,norm,importance);
}
if(mat->gi_factor != 0.0f && mat->diff_factor != 0.0f && s->pm){
ga_shade_gi(&color,s,mat,&dpos,norm);
}
if(mat->gi_sample > 0){
ga_shade_sky(&color,s,mat,&dpos,norm,importance);
}
/* pointlight shading */
n = s->light->first;
if(n && (mat->diff_factor != 0.0f || mat->spec_factor != 0.0f ||
mat->flat_factor != 0.0f )){
while(n){ /* for each light in scene */
light = (ga_light_t*)n->data;
sample = light->samples;
lsample = light->pos;
ldir = vec_sub(light->pos,dpos);
vec_fperp(&ldir,&d1,&d2);
while(sample--){ /*for each sample in light */
do{
lsample = vec_add(
vec_scale((-1.0f + 2.0f*random()*RAND_NORM)*light->radius,d1),
vec_scale((-1.0f + 2.0f*random()*RAND_NORM)*light->radius,d2));
}while(vec_len(lsample) > light->radius + 0.000001);
lsample = vec_add(light->pos,lsample); /* sampled light position */
ldir = vec_norm(vec_sub(lsample,dpos));
len = vec_len(vec_sub(light->pos,dpos)); /* distance to light */
rlen = ga_ray_length(s,dpos,ldir);
if(len < rlen){ /* light is visible */
len *= len;
len = 1.0/len;
lcolor = vec_scale(len*light->color.w,light->color);
if(mat->diff_factor != 0.0f){
ga_shade_diffuse(&color,mat,&lcolor,&ldir,norm,1.0/light->samples);
}
if(mat->spec_factor != 0.0f){
ga_shade_phong(&color,mat,&lcolor,&ldir,dir,norm,1.0/light->samples);
}
if(mat->flat_factor != 0.0f){
ga_shade_flat(&color,mat,&lcolor,1.0/light->samples);
}
}
}
n = n->next;
}
}
return color;
}