FLT_DBL
find_ortho (FLT_DBL * cc, FLT_DBL * rmat)
{
int kk;
FLT_DBL ccrot[6 * 6];
FLT_DBL ccortho[6 * 6];
FLT_DBL ccrot2[6 * 6];
FLT_DBL ccti[6 * 6];
FLT_DBL rmat_transp[9];
FLT_DBL rmat_temp[9];
FLT_DBL rmat_temp2[9];
FLT_DBL vec[3];
FLT_DBL vec2[3];
FLT_DBL dist;
FLT_DBL dista[3];
FLT_DBL qq[4], qq_best[4];
double center[4];
double range[4];
int count[4];
int qindex[4];
double inc[4];
FLT_DBL phi, theta;
FLT_DBL temp;
FLT_DBL dist_best;
/*
* Keep the compiler from complaining that this may be uninitialized.
*/
dist_best = NO_NORM;
/*
* Search over all possible orientations.
*
* Any orientation in 3-space can be specified by a unit vector,
* giving an axis to rotate around, and an angle to rotate about
* the given axis. The orientation is given with respect to some
* fixed reference orientation.
*
* Since rotating by theta degrees about (A,B,C) produces the same
* result as rotating -theta degrees about (-A,-B,-C), we only
* need to consider 180 degrees worth of angles, not 360.
*
* In this application, we are finding the orientation of an orthorhombic
* medium. Orthorhombic symmetry has three orthogonal symmetry planes,
* so any one octant defines the whole. We thus only need to search
* over rotation axes within one octant.
*
* Following the article in EDN, March 2, 1995, on page 95, author
* "Do-While Jones" (a pen name of R. David Pogge),
* "Quaternions quickly transform coordinates without error buildup",
* we use quaternions to express the rotation. The article can be read
* online here:
* http://www.reed-electronics.com/ednmag/archives/1995/030295/05df3.htm
*
* If (A,B,C) is a unit vector to rotate theta degrees about, then:
*
* q0 = Cos (theta/2)
* q1 = A * Sin(theta/2)
* q2 = B * Sin(theta/2)
* q3 = C * Sin(theta/2)
*
* so that q0^2 + q1^2 + q2^2 + q3^2 = 1. (A unit magnitude quaternion
* represents a pure rotation, with no change in scale).
*
* For our case, taking advantage of the orthorhombic symmetry to
* restrict the search space, we have:
* 0 <= A <= 1
* 0 <= B <= 1
* 0 <= C <= 1
* 0 <= theta <= 180 degrees.
* The rotation axis direction is limited to within one octant,
* and the rotation about that axis is limited to half of the full circle.
*
* In terms of quaternions, this bounds all four elements between 0 and 1,
* inclusive.
*/
/*
* How much to subdivide each quaternion axis in the original scan. These
* were somewhat arbitrarily chosen. These choices appear to be overkill,
* but that ensures we won't accidentally miss the correct result by
* insufficient sampling of the search space.
*/
/*
* We sample the rotation angle more finely than the rotation axis.
*/
count[0] = SUB_ROT;
count[1] = SUB_POS;
count[2] = SUB_POS;
count[3] = SUB_POS;
/*
* Between 0. and 1. for all 4 Q's (That is .5 +- .5.)
*/
for (kk = 0; kk < 4; kk++)
{
range[kk] = .5;
center[kk] = .5;
/*
* A number meaning "not set yet", to get us through the loop the
* first time. Needs to be much bigger than END_RES.
*/
inc[kk] = NOT_SET_YET;
}
while (inc[0] > END_RES && inc[1] > END_RES &&
inc[2] > END_RES && inc[3] > END_RES)
{
/*
* Update inc to reflect the increment for the current search
*/
for (kk = 0; kk < 4; kk++)
{
inc[kk] = (2. * range[kk]) / (FLT_DBL) (count[kk] - 1);
}
/*
* Start the 4-dimensional search. Keep track of the best result
* found so far. The distance must be non-negative; we use -1 to mean
* "not set yet".
*/
dist_best = NO_NORM;
for (qindex[3] = 0; qindex[3] < count[3]; qindex[3]++)
for (qindex[2] = 0; qindex[2] < count[2]; qindex[2]++)
for (qindex[1] = 0; qindex[1] < count[1]; qindex[1]++)
for (qindex[0] = 0; qindex[0] < count[0]; qindex[0]++)
{
/*
* Calculate the quaternion for this search point.
*/
for (kk = 0; kk < 4; kk++)
{
/*
* The term in parenthesis ranges from -1 to +1,
* inclusive, so qq ranges from (-range+center)
* to (+range + center).
*/
qq[kk] =
range[kk] *
(((FLT_DBL)
(2 * qindex[kk] -
(count[kk] -
1))) / ((FLT_DBL) (count[kk] - 1))) +
center[kk];
}
/*
* Convert from a quaternion to a rotation matrix.
* The subroutine also takes care of normalizing the
* quaternion.
*/
quaternion_to_matrix (qq, rmat);
/*
* Apply the rotation matrix to the elastic stiffness
* matrix.
*/
rotate_tensor (ccrot, cc, rmat);
/*
* Find the distance of the rotated medium from
* orthorhombic aligned with the coordinate axes.
*/
dist = ortho_distance (ccortho, ccrot);
/*
* If it's the best found so far, or the first time
* through, remember it.
*/
if (dist < dist_best || dist_best < 0.)
{
dist_best = dist;
for (kk = 0; kk < 4; kk++)
qq_best[kk] = qq[kk];
}
}
/*
* Refine for the next, finer, search. To avoid any possible problem
* caused by the optimal solution landing at an edge, we search over
* twice the distance between the two search points from the previous
* iteration.
*/
for (kk = 0; kk < 4; kk++)
{
center[kk] = qq_best[kk];
count[kk] = SUBDIVIDE;
range[kk] = inc[kk];
}
/*
* We keep refining and searching the ever finer grid until we
* achieve the required accuracy, at which point we fall out the
* bottom of the loop here.
*/
}
/*
* We've got the answer to sufficient resolution... clean it up a bit,
* then output it.
*/
/*
* Convert the best answer from a Quaternion back to a rotation matrix
*/
quaternion_to_matrix (qq_best, rmat);
/*
* To make the order of the axes unique, we sort the principal axes
* according to how well they work as a TI symmetry axis.
*
* Specifically, since after rotation the medium is canonically oriented,
* with the X, Y, and Z axes the principal axes, the INVERSE rotation
* must take the X, Y, and Z axes to the original arbitrarily oriented
* principal axes. So we first inverse-rotate a coordinate axis back to a
* principal axis. We then use vector_to_angles to give us the Euler
* angles theta and phi for the principal axis. make_rotation_matrix then
* constructs a rotation matrix that rotates that principal axis to +Z.
* We then use that matrix to rotate the tensor. We then measure its
* distance from VTI, and remember that distance.
*/
/*
* First we need to find the inverse (the same as the transpose, because
* it's _unitary_) of the rotation matrix rmat.
*/
transpose_matrix (rmat_transp, rmat);
/* Test the X axis */
vec[0] = 1.;
vec[1] = 0.;
vec[2] = 0.;
matrix_times_vector (vec2, rmat_transp, vec);
vector_to_angles (vec2, &phi, &theta);
make_rotation_matrix (theta, phi, 0., rmat_temp);
rotate_tensor (ccrot2, cc, rmat_temp);
dista[0] = ti_distance (ccti, ccrot2);
/* Test the Y axis */
vec[0] = 0.;
vec[1] = 1.;
vec[2] = 0.;
matrix_times_vector (vec2, rmat_transp, vec);
vector_to_angles (vec2, &phi, &theta);
make_rotation_matrix (theta, phi, 0., rmat_temp);
rotate_tensor (ccrot2, cc, rmat_temp);
dista[1] = ti_distance (ccti, ccrot2);
/* Test the Z axis */
vec[0] = 0.;
vec[1] = 0.;
vec[2] = 1.;
matrix_times_vector (vec2, rmat_transp, vec);
vector_to_angles (vec2, &phi, &theta);
make_rotation_matrix (theta, phi, 0., rmat_temp);
rotate_tensor (ccrot2, cc, rmat_temp);
dista[2] = ti_distance (ccti, ccrot2);
/*
* See which axis best functions as a TI symmetry axis, and make that one
* the Z axis.
*/
if (dista[2] <= dista[1] && dista[2] <= dista[0])
{
/* The Z axis is already the best. No rotation needed. */
make_rotation_matrix (0., 0., 0., rmat_temp);
}
else if (dista[1] <= dista[2] && dista[1] <= dista[0])
{
/* Rotate Y to Z */
make_rotation_matrix (0., 90., 0., rmat_temp);
temp = dista[2];
dista[2] = dista[1];
dista[1] = temp;
}
else
{
/* Rotate X to Z */
make_rotation_matrix (90., 90., -90., rmat_temp);
temp = dista[2];
dista[2] = dista[0];
dista[0] = temp;
}
/*
* Accumulate this axis-relabeling rotation (rmat_temp) onto the original
* rotation (rmat).
*/
matrix_times_matrix (rmat_temp2, rmat_temp, rmat);
/*
* Now find the next-best TI symmetry axis and make that one the Y axis.
*/
if (dista[1] <= dista[0])
{
/* Already there; do nothing. */
make_rotation_matrix (0., 0., 0., rmat_temp);
}
else
{
/* Rotate X to Y */
make_rotation_matrix (90., 0., 0., rmat_temp);
temp = dista[1];
dista[1] = dista[0];
dista[0] = temp;
}
/*
* Accumulate the new axis relabeling rotation (rmat_temp) onto the
* combined previous rotation matrix (rmat_temp2) to produce the final
* desired result, rmat. The axes should now be in sorted order.
*/
matrix_times_matrix (rmat, rmat_temp, rmat_temp2);
return dist_best;
}
void update_key_frame( player_data_t *plyr, scalar_t dt )
{
int idx;
scalar_t frac;
point_t pos;
scalar_t v;
matrixgl_t cob_mat, rot_mat;
char *root;
char *lsh;
char *rsh;
char *lhp;
char *rhp;
char *lkn;
char *rkn;
char *lank;
char *rank;
char *head;
char *neck;
char *tail;
root = get_tux_root_node();
lsh = get_tux_left_shoulder_joint();
rsh = get_tux_right_shoulder_joint();
lhp = get_tux_left_hip_joint();
rhp = get_tux_right_hip_joint();
lkn = get_tux_left_knee_joint();
rkn = get_tux_right_knee_joint();
lank = get_tux_left_ankle_joint();
rank = get_tux_right_ankle_joint();
head = get_tux_head();
neck = get_tux_neck();
tail = get_tux_tail_joint();
keyTime += dt;
for (idx = 1; idx < numFrames; idx ++) {
if ( keyTime < frames[idx].time )
break;
}
if ( idx == numFrames || numFrames == 0 ) {
set_game_mode( RACING );
return;
}
reset_scene_node( root );
reset_scene_node( lsh );
reset_scene_node( rsh );
reset_scene_node( lhp );
reset_scene_node( rhp );
reset_scene_node( lkn );
reset_scene_node( rkn );
reset_scene_node( lank );
reset_scene_node( rank );
reset_scene_node( head );
reset_scene_node( neck );
reset_scene_node( tail );
check_assertion( idx > 0, "invalid keyframe index" );
if ( fabs( frames[idx-1].time - frames[idx].time ) < EPS ) {
frac = 1.;
} else {
frac = (keyTime - frames[idx].time)
/ ( frames[idx-1].time - frames[idx].time );
}
pos.x = interp( frac, frames[idx-1].pos.x, frames[idx].pos.x );
pos.z = interp( frac, frames[idx-1].pos.z, frames[idx].pos.z );
pos.y = interp( frac, frames[idx-1].pos.y, frames[idx].pos.y );
pos.y += find_y_coord( pos.x, pos.z );
set_tux_pos( plyr, pos );
make_identity_matrix( cob_mat );
v = interp( frac, frames[idx-1].yaw, frames[idx].yaw );
rotate_scene_node( root, 'y', v );
make_rotation_matrix( rot_mat, v, 'y' );
multiply_matrices( cob_mat, cob_mat, rot_mat );
v = interp( frac, frames[idx-1].pitch, frames[idx].pitch );
rotate_scene_node( root, 'x', v );
make_rotation_matrix( rot_mat, v, 'x' );
multiply_matrices( cob_mat, cob_mat, rot_mat );
v = interp( frac, frames[idx-1].l_shldr, frames[idx].l_shldr );
rotate_scene_node( lsh, 'z', v );
v = interp( frac, frames[idx-1].r_shldr, frames[idx].r_shldr );
rotate_scene_node( rsh, 'z', v );
v = interp( frac, frames[idx-1].l_hip, frames[idx].l_hip );
rotate_scene_node( lhp, 'z', v );
v = interp( frac, frames[idx-1].r_hip, frames[idx].r_hip );
rotate_scene_node( rhp, 'z', v );
/* Set orientation */
plyr->orientation = make_quaternion_from_matrix( cob_mat );
plyr->orientation_initialized = True;
}
mat3x3
make_rotation_matrix(vec3 *v)
{
return make_rotation_matrix(v->x, v->y, v->z);
}
void update_view( player_data_t *plyr, scalar_t dt )
{
point_t view_pt;
vector_t view_dir, up_dir, vel_dir, view_vec;
scalar_t ycoord;
scalar_t course_angle;
vector_t axis;
matrixgl_t rot_mat;
vector_t y_vec;
vector_t mz_vec;
vector_t vel_proj;
quaternion_t rot_quat;
scalar_t speed;
vector_t vel_cpy;
scalar_t time_constant_mult;
vel_cpy = plyr->vel;
speed = normalize_vector( &vel_cpy );
time_constant_mult = 1.0 /
min( 1.0,
max( 0.0,
( speed - NO_INTERPOLATION_SPEED ) /
( BASELINE_INTERPOLATION_SPEED - NO_INTERPOLATION_SPEED )));
up_dir = make_vector( 0, 1, 0 );
vel_dir = plyr->vel;
normalize_vector( &vel_dir );
course_angle = get_course_angle();
switch( plyr->view.mode ) {
case TUXEYE:
{
scalar_t f = 2;
vector_t v = plyr->plane_nml;
scalar_t n = 1.;
view_pt = plyr->pos;
view_pt.x += v.x / n * 0.3;
view_pt.y += v.y / n * 0.3;
view_pt.y += 0.1;
view_pt.z += v.z / n * 0.3;
if(plyr->control.flip_factor || plyr->control.barrel_roll_factor) {
matrixgl_t mat1, mat;
vector_t right;
scalar_t n = sqrt(plyr->viewdir_for_tuxeye.x * plyr->viewdir_for_tuxeye.x + plyr->viewdir_for_tuxeye.y * plyr->viewdir_for_tuxeye.y + plyr->viewdir_for_tuxeye.z * plyr->viewdir_for_tuxeye.z);
plyr->viewdir_for_tuxeye.x /= n;
plyr->viewdir_for_tuxeye.y /= n;
plyr->viewdir_for_tuxeye.z /= n;
n = sqrt(plyr->updir_for_tuxeye.x * plyr->updir_for_tuxeye.x + plyr->updir_for_tuxeye.y * plyr->updir_for_tuxeye.y + plyr->updir_for_tuxeye.z * plyr->updir_for_tuxeye.z);
plyr->updir_for_tuxeye.x /= n;
plyr->updir_for_tuxeye.y /= n;
plyr->updir_for_tuxeye.z /= n;
right = cross_product(plyr->updir_for_tuxeye, plyr->viewdir_for_tuxeye);
make_rotation_about_vector_matrix( mat1, right, jump_from_time(plyr->control.flip_factor) * 360 );
make_rotation_about_vector_matrix( mat, plyr->viewdir_for_tuxeye, jump_from_time(plyr->control.barrel_roll_factor) * 360 );
multiply_matrices(mat, mat1, mat);
view_dir = transform_vector(mat, plyr->viewdir_for_tuxeye);
up_dir = transform_vector(mat, plyr->updir_for_tuxeye);
}
else {
view_dir = plyr->direction;
view_dir.y += 0.1;
view_dir.x = (plyr->view.dir.x * f + view_dir.x) / (f + 1);
view_dir.y = (plyr->view.dir.y * f + view_dir.y) / (f + 1);
view_dir.z = (plyr->view.dir.z * f + view_dir.z) / (f + 1);
plyr->viewdir_for_tuxeye = view_dir;
up_dir = plyr->plane_nml;
up_dir.x = (plyr->view.up.x * f + up_dir.x) / (f + 1);
up_dir.y = (plyr->view.up.y * f + up_dir.y) / (f + 1);
up_dir.z = (plyr->view.up.z * f + up_dir.z) / (f + 1);
plyr->updir_for_tuxeye = up_dir;
}
break;
}
case BEHIND:
{
/* Camera-on-a-string mode */
/* Construct vector from player to camera */
view_vec = make_vector( 0,
sin( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE +
PLAYER_ANGLE_IN_CAMERA ) ),
cos( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE +
PLAYER_ANGLE_IN_CAMERA ) ) );
view_vec = scale_vector( CAMERA_DISTANCE, view_vec );
y_vec = make_vector( 0.0, 1.0, 0.0 );
mz_vec = make_vector( 0.0, 0.0, -1.0 );
vel_proj = project_into_plane( y_vec, vel_dir );
normalize_vector( &vel_proj );
/* Rotate view_vec so that it places the camera behind player */
rot_quat = make_rotation_quaternion( mz_vec, vel_proj );
view_vec = rotate_vector( rot_quat, view_vec );
/* Construct view point */
view_pt = move_point( plyr->pos, view_vec );
/* Make sure view point is above terrain */
ycoord = find_y_coord( view_pt.x, view_pt.z );
if ( view_pt.y < ycoord + MIN_CAMERA_HEIGHT ) {
view_pt.y = ycoord + MIN_CAMERA_HEIGHT;
}
/* Interpolate view point */
if ( plyr->view.initialized ) {
/* Interpolate twice to get a second-order filter */
int i;
for (i=0; i<2; i++) {
view_pt =
interpolate_view_pos( plyr->pos, plyr->pos,
MAX_CAMERA_PITCH, plyr->view.pos,
view_pt, CAMERA_DISTANCE, dt,
BEHIND_ORBIT_TIME_CONSTANT *
time_constant_mult );
}
}
/* Make sure interpolated view point is above terrain */
ycoord = find_y_coord( view_pt.x, view_pt.z );
if ( view_pt.y < ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT ) {
view_pt.y = ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT;
}
/* Construct view direction */
view_vec = subtract_points( view_pt, plyr->pos );
axis = cross_product( y_vec, view_vec );
normalize_vector( &axis );
make_rotation_about_vector_matrix( rot_mat, axis,
PLAYER_ANGLE_IN_CAMERA );
view_dir = scale_vector( -1.0,
transform_vector( rot_mat, view_vec ) );
/* Interpolate orientation of camera */
if ( plyr->view.initialized ) {
/* Interpolate twice to get a second-order filter */
int i;
for (i=0; i<2; i++) {
interpolate_view_frame( plyr->view.up, plyr->view.dir,
&up_dir, &view_dir, dt,
BEHIND_ORIENT_TIME_CONSTANT );
up_dir = make_vector( 0.0, 1.0, 0.0 );
}
}
break;
}
case FOLLOW:
{
/* Camera follows player (above and behind) */
up_dir = make_vector( 0, 1, 0 );
/* Construct vector from player to camera */
view_vec = make_vector( 0,
sin( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE +
PLAYER_ANGLE_IN_CAMERA ) ),
cos( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE +
PLAYER_ANGLE_IN_CAMERA ) ) );
view_vec = scale_vector( CAMERA_DISTANCE, view_vec );
y_vec = make_vector( 0.0, 1.0, 0.0 );
mz_vec = make_vector( 0.0, 0.0, -1.0 );
vel_proj = project_into_plane( y_vec, vel_dir );
normalize_vector( &vel_proj );
/* Rotate view_vec so that it places the camera behind player */
rot_quat = make_rotation_quaternion( mz_vec, vel_proj );
view_vec = rotate_vector( rot_quat, view_vec );
/* Construct view point */
view_pt = move_point( plyr->pos, view_vec );
/* Make sure view point is above terrain */
ycoord = find_y_coord( view_pt.x, view_pt.z );
if ( view_pt.y < ycoord + MIN_CAMERA_HEIGHT ) {
view_pt.y = ycoord + MIN_CAMERA_HEIGHT;
}
/* Interpolate view point */
if ( plyr->view.initialized ) {
/* Interpolate twice to get a second-order filter */
int i;
for ( i=0; i<2; i++ ) {
view_pt =
interpolate_view_pos( plyr->view.plyr_pos, plyr->pos,
MAX_CAMERA_PITCH, plyr->view.pos,
view_pt, CAMERA_DISTANCE, dt,
FOLLOW_ORBIT_TIME_CONSTANT *
time_constant_mult );
}
}
/* Make sure interpolate view point is above terrain */
ycoord = find_y_coord( view_pt.x, view_pt.z );
if ( view_pt.y < ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT ) {
view_pt.y = ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT;
}
/* Construct view direction */
view_vec = subtract_points( view_pt, plyr->pos );
axis = cross_product( y_vec, view_vec );
normalize_vector( &axis );
make_rotation_about_vector_matrix( rot_mat, axis,
PLAYER_ANGLE_IN_CAMERA );
view_dir = scale_vector( -1.0,
transform_vector( rot_mat, view_vec ) );
/* Interpolate orientation of camera */
if ( plyr->view.initialized ) {
/* Interpolate twice to get a second-order filter */
int i;
for ( i=0; i<2; i++ ) {
interpolate_view_frame( plyr->view.up, plyr->view.dir,
&up_dir, &view_dir, dt,
FOLLOW_ORIENT_TIME_CONSTANT );
up_dir = make_vector( 0.0, 1.0, 0.0 );
}
}
break;
}
case ABOVE:
{
/* Camera always uphill of player */
up_dir = make_vector( 0, 1, 0 );
/* Construct vector from player to camera */
view_vec = make_vector( 0,
sin( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE+
PLAYER_ANGLE_IN_CAMERA ) ),
cos( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE+
PLAYER_ANGLE_IN_CAMERA ) ) );
view_vec = scale_vector( CAMERA_DISTANCE, view_vec );
/* Construct view point */
view_pt = move_point( plyr->pos, view_vec );
/* Make sure view point is above terrain */
ycoord = find_y_coord( view_pt.x, view_pt.z );
if ( view_pt.y < ycoord + MIN_CAMERA_HEIGHT ) {
view_pt.y = ycoord + MIN_CAMERA_HEIGHT;
}
/* Construct view direction */
view_vec = subtract_points( view_pt, plyr->pos );
make_rotation_matrix( rot_mat, PLAYER_ANGLE_IN_CAMERA, 'x' );
view_dir = scale_vector( -1.0,
transform_vector( rot_mat, view_vec ) );
break;
}
default:
code_not_reached();
}
/* Create view matrix */
plyr->view.pos = view_pt;
plyr->view.dir = view_dir;
plyr->view.up = up_dir;
plyr->view.plyr_pos = plyr->pos;
plyr->view.initialized = True;
setup_view_matrix( plyr );
}
//----------------------------------------------------------------------
void Wannier_method::optimize_rotation(Array3 <dcomplex> & eikr,
Array2 <doublevar> & R ) {
int norb=eikr.GetDim(1);
Array2 <doublevar> gvec(3,3);
sys->getPrimRecipLattice(gvec);
Array1 <doublevar> gnorm(3);
gnorm=0;
for(int d=0; d < 3; d++) {
for(int d1=0; d1 < 3; d1++) {
gnorm(d)+=gvec(d,d1)*gvec(d,d1);
}
gnorm(d)=sqrt(gnorm(d));
}
for(int i=0; i< norb; i++) {
cout << "rloc2 " << i << " ";
for(int d=0; d< 3; d++) {
cout << -log(norm(eikr(d,i,i)))/(gnorm(d)*gnorm(d)) << " ";
}
cout << endl;
}
Array2 <doublevar> Rgen(norb,norb),Rgen_save(norb,norb);
//R(norb,norb);
R.Resize(norb,norb);
//Array2 <dcomplex> tmp(norb,norb),tmp2(norb,norb);
//Shake up the angles, since often the original orbitals
//are at a maximum and derivatives are zero.
Array2 <doublevar> deriv(norb,norb);
Rgen=0.0;
for(int ii=0; ii< norb; ii++) {
for(int jj=ii+1; jj< norb; jj++) {
Rgen(ii,jj)=rng.gasdev()*pi*shake;
}
}
for(int step=0; step < max_opt_steps; step++) {
doublevar fbase=evaluate_local(eikr,Rgen,R);
for(int ii=0; ii <norb; ii++) {
cout << "deriv ";
for(int jj=ii+1; jj < norb; jj++) {
doublevar save_rgeniijj=Rgen(ii,jj);
doublevar h=1e-6;
Rgen(ii,jj)+=h;
doublevar func=evaluate_local(eikr,Rgen,R);
deriv(ii,jj)=(func-fbase)/h;
Rgen(ii,jj)=save_rgeniijj;
cout << deriv(ii,jj) << " ";
}
cout << endl;
}
doublevar rloc_thresh=0.0001;
Rgen_save=Rgen;
doublevar best_func=1e99, best_tstep=0.0;
doublevar bracket_tstep=0.0;
doublevar last_func=fbase;
for(doublevar tstep=0.01; tstep < 20.0; tstep*=2.0) {
doublevar func=eval_tstep(eikr,Rgen,Rgen_save,deriv,tstep,R);
cout << "tstep " << tstep << " func " << func << endl;
if(func > fbase or func > last_func) {
bracket_tstep=tstep;
break;
}
else last_func=func;
}
cout << "bracket_tstep " << bracket_tstep << endl;
doublevar resphi=2.-(1.+sqrt(5.))/2.;
doublevar a=0, b=resphi*bracket_tstep, c=bracket_tstep;
doublevar af=fbase, bf=eval_tstep(eikr,Rgen,Rgen_save,deriv,b,R), cf=eval_tstep(eikr,Rgen,Rgen_save,deriv,bracket_tstep,R);
cout << "first step a,b,c " << a << " " << b << " " << c
<< " funcs " << af << " " << bf << " " << cf << endl;
for(int it=0; it < 20; it++) {
doublevar d,df;
if( (c-b) > (b-a))
d=b+resphi*(c-b);
else
d=b-resphi*(b-a);
df=eval_tstep(eikr,Rgen,Rgen_save,deriv,d,R);
if(df < bf) {
if( (c-b) > (b-a) ) {
a=b;
af=bf;
b=d;
bf=df;
}
else {
c=b;
cf=bf;
b=d;
bf=df;
}
}
else {
if( (c-b) > (b-a) ) {
c=d;
cf=df;
}
else {
a=d;
af=df;
}
}
cout << "step " << it << " a,b,c " << a << " " << b << " " << c
<< " funcs " << af << " " << bf << " " << cf << endl;
}
best_tstep=b;
/*
bool made_move=false;
while (!made_move) {
for(doublevar tstep=0.00; tstep < max_tstep; tstep+=0.1*max_tstep) {
for(int ii=0; ii< norb;ii++) {
for(int jj=ii+1; jj < norb; jj++) {
Rgen(ii,jj)=Rgen_save(ii,jj)-tstep*deriv(ii,jj);
}
}
doublevar func=evaluate_local(eikr,Rgen,R);
if(func < best_func) {
best_func=func;
best_tstep=tstep;
}
cout << " tstep " << tstep << " " << func << endl;
}
if(abs(best_tstep) < 0.2*max_tstep)
max_tstep*=0.5;
else if(abs(best_tstep-max_tstep) < 1e-14)
max_tstep*=2.0;
else made_move=true;
}
*/
for(int ii=0; ii< norb; ii++) {
for(int jj=ii+1; jj < norb; jj++) {
Rgen(ii,jj)=Rgen_save(ii,jj)-best_tstep*deriv(ii,jj);
}
}
doublevar func2=evaluate_local(eikr,Rgen,R);
doublevar max_change=0;
for(int ii=0; ii < norb; ii++) {
for(int jj=ii+1; jj< norb; jj++) {
doublevar change=abs(Rgen(ii,jj)-Rgen_save(ii,jj));
if(change > max_change) max_change=change;
}
}
cout << "tstep " << best_tstep << " rms " << sqrt(func2) << " bohr max change " << max_change <<endl;
doublevar threshold=0.0001;
if(max_change < threshold) break;
if(abs(best_func-fbase) < rloc_thresh) break;
/*
bool moved=false;
for(int ii=0; ii< norb; ii++) {
for(int jj=ii+1; jj< norb; jj++) {
doublevar save_rgeniijj=Rgen(ii,jj);
doublevar best_del=0;
doublevar best_f=1e99;
for(doublevar del=-0.5; del < 0.5; del+=0.05) {
cout << "############ for del = " << del << endl;
Rgen(ii,jj)=save_rgeniijj+del;
doublevar func=evaluate_local(eikr,Rgen,R);
if(func < best_f) {
best_f=func;
best_del=del;
}
cout << "func " << func << endl;
}
Rgen(ii,jj)=save_rgeniijj+best_del;
if(abs(best_del) > 1e-12) moved=true;
}
}
if(!moved) break;
*/
}
make_rotation_matrix(Rgen,R);
}
ExperimentalApp() : GLFWApp(1280, 800, "Geometric Algorithm Development App")
{
glfwSwapInterval(0);
igm.reset(new gui::ImGuiManager(window));
gui::make_dark_theme();
fixedTimer.start();
lights[0] = {{0, 10, -10}, {0, 0, 1}};
lights[1] = {{0, 10, 10}, {0, 1, 0}};
int width, height;
glfwGetWindowSize(window, &width, &height);
glViewport(0, 0, width, height);
grid = RenderableGrid(1, 100, 100);
cameraController.set_camera(&camera);
camera.look_at({0, 2.5, -2.5}, {0, 2.0, 0});
simpleShader = make_watched_shader(shaderMonitor, "../assets/shaders/simple_vert.glsl", "assets/shaders/simple_frag.glsl");
normalDebugShader = make_watched_shader(shaderMonitor, "../assets/shaders/normal_debug_vert.glsl", "assets/shaders/normal_debug_frag.glsl");
Renderable debugAxis = Renderable(make_axis(), false, GL_LINES);
debugAxis.pose = Pose(float4(0, 0, 0, 1), float3(0, 1, 0));
debugModels.push_back(std::move(debugAxis));
// Initial supershape settings
supershape = Renderable(make_supershape_3d(16, 5, 7, 4, 12));
supershape.pose.position = {0, 2, -2};
// Initialize PTF stuff
{
std::array<float3, 4> controlPoints = {float3(0.0f, 0.0f, 0.0f), float3(0.667f, 0.25f, 0.0f), float3(1.33f, 0.25f, 0.0f), float3(2.0f, 0.0f, 0.0f)};
ptf = make_parallel_transport_frame_bezier(controlPoints, 32);
for (int i = 0; i < ptf.size(); ++i)
{
Renderable p = Renderable(make_cube());
ptfBoxes.push_back(std::move(p));
}
}
// Initialize Parabolic pointer stuff
{
// Set up the ground plane used as a nav mesh for the parabolic pointer
worldSurface = make_plane(48, 48, 96, 96);
for (auto & p : worldSurface.vertices)
{
float4x4 model = make_rotation_matrix({1, 0, 0}, -ANVIL_PI / 2);
p = transform_coord(model, p);
}
worldSurfaceRenderable = Renderable(worldSurface);
parabolicPointer = make_parabolic_pointer(worldSurface, params);
}
// Initialize objects for ballistic trajectory tests
{
turret.source = Renderable(make_tetrahedron());
turret.source.pose = Pose({-5, 2, 5});
turret.target = Renderable(make_cube());
turret.target.pose = Pose({0, 0, 40});
turret.bullet = Renderable(make_sphere(1.0f));
}
float4x4 tMat = mul(make_translation_matrix({3, 4, 5}), make_rotation_matrix({0, 0, 1}, ANVIL_PI / 2));
auto p = make_pose_from_transform_matrix(tMat);
std::cout << tMat << std::endl;
std::cout << p << std::endl;
gl_check_error(__FILE__, __LINE__);
}
