/* Create a matrix where the eye is at the xyz basis looking down the -z axis */
tmat viewMat(const vec3 &eye, const vec3 &gaze, const vec3 &up)
{
tmat mat = identityMatrix();
// orthonormal basis from params
vec3 w = -makeUnitVector(gaze);
vec3 u = makeUnitVector(cross(up, w)); // get a vector orthogonal to up and w
vec3 v = cross(w, u); // normalized since the others already are
// rotate the orthonormal basis to the xyz basis
mat.m[0][0] = u.x;
mat.m[0][1] = u.y;
mat.m[0][2] = u.z;
mat.m[1][0] = v.x;
mat.m[1][1] = v.y;
mat.m[1][2] = v.z;
mat.m[2][0] = w.x;
mat.m[2][1] = w.y;
mat.m[2][2] = w.z;
// translate the matrix to the xyz origin
tmat move = identityMatrix();
move.m[0][3] = -(eye.x);
move.m[1][3] = -(eye.y);
move.m[2][3] = -(eye.z);
mat *= move;
return mat;
}
/* Create a tmat that rotates things by angle radians in the direction given by axis */
tmat rotate(const vec3 &axis, float angle)
{
vec3 naxis = makeUnitVector(axis); // unit vect in direction of axis
tmat mat;
float x = naxis.x;
float y = naxis.y;
float z = naxis.z;
float cosine = cos(angle);
float sine = sin(angle);
float t = 1.0f - cosine;
mat.m[0][0] = t * x * x + cosine;
mat.m[0][1] = t * x * y - sine * z;
mat.m[0][2] = t * x * z + sine * y;
mat.m[0][3] = 0.0f;
mat.m[1][0] = t * x * y + sine * z;
mat.m[1][1] = t * y * y + cosine;
mat.m[1][2] = t * y * z - sine * x;
mat.m[1][3] = 0.0f;
mat.m[2][0] = t * x * z - sine * y;
mat.m[2][1] = t * y * z + sine * x;
mat.m[2][2] = t * z * z + cosine;
mat.m[2][3] = 0.0f;
mat.m[3][0] = 0.0f;
mat.m[3][1] = 0.0f;
mat.m[3][2] = 0.0f;
mat.m[3][3] = 1.0f;
return mat;
}
void icm_Abst::gradientDescent_step(){
if(failedGA_counts > 500){return;}
double org_llk = sum_llk();
backupCH = baseCH;
baseCH_2_baseS();
baseS_2_baseP();
numeric_dobs_dp(true);
int k = base_p_derv.size();
prop_p.resize(k);
double prop_mean = 0;
int act_sum = 0;
double new_llk;
vector<bool> isActive(k);
for(int i = 0; i < k; i++){
if(baseP[i] > 0 && !ISNAN(base_p_derv[i]) ){
isActive[i] = true;
act_sum++;
}
else { isActive[i] = false; }
}
for(int i = 0; i < k; i++){
if(isActive[i]){ prop_mean += base_p_derv[i]; }
}
prop_mean = prop_mean / act_sum;
for(int i = 0; i < k; i++){
if(isActive[i]){ prop_p[i] = base_p_derv[i] - prop_mean;}
else {prop_p[i] = 0.0;}
}
makeUnitVector(prop_p);
double scale_max = getMaxScaleSize(baseP, prop_p);
for(int i = 0; i < k; i++){ prop_p[i] *= -1.0; }
scale_max = min(scale_max, getMaxScaleSize(baseP, prop_p));
for(int i = 0; i < k; i++){ prop_p[i] *= -1.0; }
double delta_val = scale_max/2.0;
delta_val = min(delta_val, h);
delta_val = delta_val/10.0;
double analytic_dd = directional_derv(base_p_derv, prop_p);
if(delta_val == 0){
failedGA_counts++;
baseCH = backupCH;
new_llk = sum_llk();
return;
}
add_vec(delta_val, prop_p, baseP);
double llk_h = llk_from_p();
add_vec(-2.0 * delta_val, prop_p, baseP);
double llk_l = llk_from_p();
add_vec(delta_val, prop_p, baseP);
double llk_0 = llk_from_p();
double d1 = ( llk_h - llk_l ) / ( 2 * delta_val );
double d2 = (llk_h + llk_l - 2.0 * llk_0 ) / (delta_val * delta_val);
if(iter % 2 ==0){ d1 = analytic_dd; }
delta_val = -d1/d2;
if(!(delta_val > 0)){
failedGA_counts++;
baseCH = backupCH;
new_llk = sum_llk();
return;
}
if(ISNAN(delta_val)){
failedGA_counts++;
baseCH= backupCH;
new_llk = sum_llk();
return;
}
scale_max = getMaxScaleSize(baseP, prop_p);
delta_val = min( delta_val, scale_max );
add_vec(delta_val, prop_p, baseP);
new_llk = llk_from_p();
mult_vec(-1.0, prop_p);
int tries = 0;
double this_delta = delta_val;
while(tries < 5 && new_llk < llk_0){
tries++;
this_delta = this_delta/2;
add_vec(this_delta, prop_p, baseP);
new_llk = llk_from_p();
}
if(new_llk < llk_0){
failedGA_counts++;
baseCH = backupCH;
new_llk = sum_llk(); //Should NOT be llk_from_p(), since we are resetting the CH
return;
}
if(org_llk > new_llk){
failedGA_counts++;
baseCH = backupCH;
new_llk = sum_llk();
}
// Rprintf("change in llk in CGA step = %f\n", new_llk - org_llk);
}
void icm_Abst::experimental_step(){
if(failedGA_counts > 500){return;}
double org_llk = sum_llk();
backupCH = baseCH;
baseCH_2_baseS();
baseS_2_baseP();
numeric_dobs2_d2p();
int k = base_p_derv.size();
prop_p.resize(k);
double prop_mean = 0;
int act_sum = 0;
double new_llk;
vector<bool> isActive(k);
for(int i = 0; i < k; i++){
if(baseP[i] > 0 && !ISNAN(base_p_derv[i]) && base_p_2ndDerv[i] < -0.001){
isActive[i] = true;
act_sum++;
}
else { isActive[i] = false; }
}
for(int i = 0; i < k; i++){
if(isActive[i]){ prop_mean += -base_p_derv[i]/base_p_2ndDerv[i]; }
}
prop_mean = prop_mean / act_sum;
for(int i = 0; i < k; i++){
if(isActive[i]){ prop_p[i] = -base_p_derv[i]/base_p_2ndDerv[i] - prop_mean;}
else {prop_p[i] = 0.0;}
}
makeUnitVector(prop_p);
double scale_max = getMaxScaleSize(baseP, prop_p);
for(int i = 0; i < k; i++){ prop_p[i] *= -1.0; }
scale_max = min(scale_max, getMaxScaleSize(baseP, prop_p));
for(int i = 0; i < k; i++){ prop_p[i] *= -1.0; }
double delta_val = scale_max/2.0;
delta_val = min(delta_val, h);
delta_val = delta_val/10.0;
// double analytic_dd = directional_derv(base_p_derv, prop_p);
if(delta_val == 0){
failedGA_counts++;
baseCH = backupCH;
new_llk = sum_llk();
Rprintf("Exit 1\n");
return;
}
add_vec(delta_val, prop_p, baseP);
double llk_h = llk_from_p();
add_vec(-2.0 * delta_val, prop_p, baseP);
double llk_l = llk_from_p();
add_vec(delta_val, prop_p, baseP);
double llk_0 = llk_from_p();
double d1 = ( llk_h - llk_l ) / ( 2 * delta_val );
double d2 = (llk_h + llk_l - 2.0 * llk_0 ) / (delta_val * delta_val);
delta_val = -d1/d2;
if(ISNAN(delta_val)){
failedGA_counts++;
baseCH= backupCH;
new_llk = sum_llk();
Rprintf("warning: delta_val is nan in GA step. llk_h = %f, llk_l = %f, llk_0 = %f, scale_max = %f\n",
llk_h, llk_l, llk_0, scale_max);
Rprintf("Exit 3\n");
return;
}
scale_max = getMaxScaleSize(baseP, prop_p);
delta_val = min( delta_val, scale_max );
add_vec(delta_val, prop_p, baseP);
new_llk = llk_from_p();
mult_vec(-1.0, prop_p);
int tries = 0;
double this_delta = delta_val;
while(tries < 5 && new_llk < llk_0){
tries++;
this_delta = this_delta/2;
add_vec(this_delta, prop_p, baseP);
new_llk = llk_from_p();
}
if(new_llk < llk_0){
failedGA_counts++;
baseCH = backupCH;
new_llk = sum_llk(); //Should NOT be llk_from_p(), since we are resetting the CH
Rprintf("Exit 4\n");
return;
}
if(org_llk > new_llk){
failedGA_counts++;
baseCH = backupCH;
new_llk = sum_llk();
}
}
