/* ----------------------------- MNI Header -----------------------------------
@NAME : procrustes
@INPUT : npoints - number of input point pairs
ndim - number of dimensions for each point
Apoints - Matrix of point set 1 (in zero offset
form). The dimensions of this matrix should be defined
to be 1 to npoints and 1 to ndim (when calling the numerical
recipes routine matrix).
Bpoints - Matrix of point set 2 (in zero offset
form). The dimensions of this matrix should be defined
to be 1 to npoints and 1 to ndim (when calling the numerical
recipes routine matrix).
@OUTPUT : translation - zero offset vector (1 to ndim) that
specifies the translation to be applied to Bpoints to line
up the centroid with that of Apoints. Calling routine must
allocate space for this vector.
centre_of_rotation - zero offset vector (1 to ndim) that
specifies the centre of rotation and scaling (this is
in fact only the centroid of Apoints). Calling routine must
allocate space for this vector.
rotation - zero offset matrix (1 to ndim by 1 to ndim)
to rotate translated Bpoints so that they line up with
Apoints. Calling routine must allocate space for this
matrix.
scale - Scalar value giving global scaling to be applied to
translated and rotated Bpoints to match Apoints.
@RETURNS : (nothing)
@DESCRIPTION: Calculates n-dimensional linear transformation from one set
of points to another, minimizing distance between equivalent
points. Transformation from Bpoints to Apoints is calculated.
@METHOD : See Matrix Computations, Golub and Van Loan, pp. 425-426 and
paper by Sibson, Robin, J.R.Statist.Soc. B(1978), Vol. 40,
No. 2, pp 234-238.
Steps of calculations are as follows :
1) Calculate translation that aligns the centroids of the
two point sets.
2) Calculate rotation/reflexion that minimizes residual.
3) Calculate scaling of points to minimize residual.
The process can be broken into independent steps because the
best translation aligns centroids independently of the choice
of rotation/reflexion and scaling and the best rotation/reflexion
can be found independently of scale (after the best translation
has been found). (See Sibson for more).
@GLOBALS : (none)
@CALLS : calc_centroid
translate
transpose
matrix_multiply
svdcmp (zero offset)
trace
@CREATED : Long time ago (Sean Marrett)
@MODIFIED : Some time later (Shyan Ku)
Feb. 26, 1990 (Weiqian Dai)
January 30, 1992 (Peter Neelin)
- complete rewrite for roughly NIL-abiding code. Modified
name and calling parameters.
---------------------------------------------------------------------------- */
void procrustes(int npoints, int ndim,
float **Apoints, float **Bpoints,
float *translation, float *centre_of_rotation,
float **rotation, float *scale)
{
int i;
float *Atranslation, *Btranslation, *svd_W;
float **Ashift, **Bshift, **Atranspose, **Btranspose;
float **svd_U, **svd_V, **svd_VT;
float **Brotated, **product;
float trace1, trace2;
/* Get the vectors for centroids */
Atranslation=vector(1,ndim);
Btranslation=vector(1,ndim);
svd_W=vector(1,ndim);
/* Get various matrices */
Ashift=matrix(1,npoints,1,ndim);
Bshift=matrix(1,npoints,1,ndim);
Atranspose=matrix(1,ndim,1,npoints);
Btranspose=matrix(1,ndim,1,npoints);
svd_U=matrix(1,ndim,1,ndim);
svd_V=matrix(1,ndim,1,ndim);
svd_VT=matrix(1,ndim,1,ndim);
Brotated=matrix(1,npoints,1,ndim);
product=matrix(1,npoints,1,npoints);
/* Calculate the centroids, remove them from A and B points and
save the translation */
calc_centroid(npoints, ndim, Apoints, centre_of_rotation);
for (i=1; i<=ndim; i++) Atranslation[i] = -centre_of_rotation[i];
translate(npoints, ndim, Apoints, Atranslation, Ashift);
calc_centroid(npoints, ndim, Bpoints, Btranslation);
for (i=1; i<=ndim; i++) Btranslation[i] *= -1;
translate(npoints, ndim, Bpoints, Btranslation, Bshift);
for (i=1; i<=ndim; i++) translation[i] = Btranslation[i] - Atranslation[i];
/* Calculate the rotation/reflexion matrix */
transpose(npoints, ndim, Bshift, Btranspose);
matrix_multiply(ndim, npoints, ndim, Btranspose, Ashift, svd_U);
svdcmp(svd_U, ndim, ndim, svd_W, svd_V);
transpose(ndim, ndim, svd_V, svd_VT);
matrix_multiply(ndim, ndim, ndim, svd_U, svd_VT, rotation);
/* Calculate the scale */
matrix_multiply(npoints, ndim, ndim, Bshift, rotation, Brotated);
transpose(npoints, ndim, Ashift, Atranspose);
matrix_multiply(npoints, ndim, npoints, Brotated, Atranspose, product);
trace1 = trace(npoints, product);
matrix_multiply(npoints, ndim, npoints, Bshift, Btranspose, product);
trace2 = trace(npoints, product);
if (trace2 != 0.0) {
*scale = trace1 / trace2;
}
else {
*scale = 0.0;
}
/* transpose back the rotation matrix */
transpose(ndim, ndim, rotation, rotation);
/* Free vectors */
free_vector(Atranslation,1,ndim);
free_vector(Btranslation,1,ndim);
free_vector(svd_W,1,ndim);
/* Free matrices */
free_matrix(Ashift,1,npoints,1,ndim);
free_matrix(Bshift,1,npoints,1,ndim);
free_matrix(Atranspose,1,ndim,1,npoints);
free_matrix(Btranspose,1,ndim,1,npoints);
free_matrix(svd_U,1,ndim,1,ndim);
free_matrix(svd_V,1,ndim,1,ndim);
free_matrix(svd_VT,1,ndim,1,ndim);
free_matrix(Brotated,1,npoints,1,ndim);
free_matrix(product,1,npoints,1,npoints);
}
void simplex_optimization
(
vfp nmodel, /* pointer to noise model */
vfp smodel, /* pointer to signal model */
int r, /* number of parameters in the noise model */
int p, /* number of parameters in the signal model */
float * min_nconstr, /* minimum parameter constraints for noise model */
float * max_nconstr, /* maximum parameter constraints for noise model */
float * min_sconstr, /* minimum parameter constraints for signal model */
float * max_sconstr, /* maximum parameter constraints for signal model */
int nabs, /* use absolute constraints for noise parameters */
int ts_length, /* length of time series array */
float ** x_array, /* independent variable matrix */
float * ts_array, /* observed time series */
float * par_rdcd, /* estimated parameters for the reduced model */
float * parameters, /* estimated parameters */
float * sse /* error sum of squares */
)
{
const int MAX_ITERATIONS = 50; /* maximum number of iterations */
const int MAX_RESTARTS = 5; /* maximum number of restarts */
const float EXPANSION_COEF = 2.0; /* expansion coefficient */
const float REFLECTION_COEF = 1.0; /* reflection coefficient */
const float CONTRACTION_COEF = 0.5; /* contraction coefficient */
const float TOLERANCE = 1.0e-4; /* solution convergence tolerance */
float ** simplex = NULL; /* the simplex itself */
float * centroid = NULL; /* center of mass of the simplex */
float * response = NULL; /* error sum of squares at each vertex */
float * step_size = NULL; /* controls random placement of new vertex */
float * test1 = NULL; /* test vertex */
float * test2 = NULL; /* test vertex */
float resp1, resp2; /* error sum of squares for test vertex */
int i; /* vertex index */
int worst; /* index of worst vertex in simplex */
int best; /* index of best vertex in simplex */
int next; /* index of next-to-worst vertex in simplex */
int num_iter; /* number of simplex algorithm iterations */
int num_restarts; /* number of restarts of simplex algorithm */
int done; /* boolean for search finished */
float fit; /* array of fitted time series values */
int dimension; /* dimension of parameter space */
/*----- dimension of parameter space -----*/
dimension = r + p;
/*----- allocate memory -----*/
allocate_arrays (dimension, &simplex, ¢roid, &response, &step_size,
&test1, &test2);
/*----- initialization for simplex algorithm -----*/
initialize_simplex (dimension, nmodel, smodel, r, p, nabs,
min_nconstr, max_nconstr, min_sconstr, max_sconstr,
par_rdcd, parameters, simplex, response, step_size,
ts_length, x_array, ts_array);
/* start loop to do simplex optimization */
num_iter = 0;
num_restarts = 0;
done = 0;
while (!done)
{
/*----- find the worst vertex and compute centroid of remaining simplex,
discarding the worst vertex -----*/
eval_vertices (dimension, response, &worst, &next, &best);
calc_centroid (dimension, simplex, worst, centroid);
/*----- reflect the worst point through the centroid -----*/
calc_reflection (dimension, simplex, centroid, worst,
REFLECTION_COEF, test1);
resp1 = calc_sse (nmodel, smodel, r, p, nabs, min_nconstr, max_nconstr,
min_sconstr, max_sconstr, par_rdcd, test1,
ts_length, x_array, ts_array);
/*----- test the reflection against the best vertex and expand it if the
reflection is better. if not, keep the reflection -----*/
if (resp1 < response[best])
{
/*----- try expanding -----*/
calc_reflection (dimension, simplex, centroid, worst,
EXPANSION_COEF, test2);
resp2 = calc_sse (nmodel, smodel, r, p, nabs,
min_nconstr, max_nconstr,
min_sconstr, max_sconstr, par_rdcd, test2,
ts_length, x_array, ts_array);
if (resp2 <= resp1) /* keep expansion */
replace (dimension, simplex, response, worst, test2, resp2);
else /* keep reflection */
replace (dimension, simplex, response, worst, test1, resp1);
}
else if (resp1 < response[next])
{
/*----- new response is between the best and next worst
so keep reflection -----*/
replace (dimension, simplex, response, worst, test1, resp1);
}
else
{
/*----- try contraction -----*/
if (resp1 >= response[worst])
calc_reflection (dimension, simplex, centroid, worst,
-CONTRACTION_COEF, test2);
else
calc_reflection (dimension, simplex, centroid, worst,
CONTRACTION_COEF, test2);
resp2 = calc_sse (nmodel, smodel, r, p, nabs,
min_nconstr, max_nconstr,
min_sconstr, max_sconstr, par_rdcd, test2,
ts_length, x_array, ts_array);
/*---- test the contracted response against the worst response ----*/
if (resp2 > response[worst])
{
/*----- new contracted response is worse, so decrease step size
and restart -----*/
num_iter = 0;
num_restarts += 1;
restart (dimension, nmodel, smodel, r, p, nabs,
min_nconstr, max_nconstr, min_sconstr, max_sconstr,
par_rdcd, simplex, response, step_size,
ts_length, x_array, ts_array);
}
else /*----- keep contraction -----*/
replace (dimension, simplex, response, worst, test2, resp2);
}
/*----- test to determine when to stop.
first, check the number of iterations -----*/
num_iter += 1; /*----- increment iteration counter -----*/
if (num_iter >= MAX_ITERATIONS)
{
/*----- restart with smaller steps -----*/
num_iter = 0;
num_restarts += 1;
restart (dimension, nmodel, smodel, r, p, nabs,
min_nconstr, max_nconstr, min_sconstr, max_sconstr,
par_rdcd, simplex, response, step_size,
ts_length, x_array, ts_array);
}
/*----- limit the number of restarts -----*/
if (num_restarts == MAX_RESTARTS) done = 1;
/*----- compare relative standard deviation of vertex responses
against a defined tolerance limit -----*/
fit = calc_good_fit (dimension, response);
if (fit <= TOLERANCE) done = 1;
/*----- if done, copy the best solution to the output array -----*/
if (done)
{
eval_vertices (dimension, response, &worst, &next, &best);
for (i = 0; i < dimension; i++)
parameters[i] = simplex[best][i];
*sse = response[best];
}
} /*----- while (!done) -----*/
deallocate_arrays (dimension, &simplex, ¢roid, &response, &step_size,
&test1, &test2);
}
void simplex_optimization (float * parameters, float * sse)
{
const int MAX_ITERATIONS = 100;
const int MAX_RESTARTS = 25;
const float EXPANSION_COEF = 2.0;
const float REFLECTION_COEF = 1.0;
const float CONTRACTION_COEF = 0.5;
const float TOLERANCE = 1.0e-10;
float ** simplex = NULL;
float * centroid = NULL;
float * response = NULL;
float * step_size = NULL;
float * test1 = NULL;
float * test2 = NULL;
float resp1, resp2;
int i, worst, best, next;
int num_iter, num_restarts;
int done;
float fit;
allocate_arrays (&simplex, ¢roid, &response, &step_size, &test1, &test2);
simplex_initialize (parameters, simplex, response, step_size);
/* start loop to do simplex optimization */
num_iter = 0;
num_restarts = 0;
done = 0;
while (!done)
{
/* find the worst vertex and compute centroid of remaining simplex,
discarding the worst vertex */
eval_vertices (response, &worst, &next, &best);
calc_centroid (simplex, worst, centroid);
/* reflect the worst point through the centroid */
calc_reflection (simplex, centroid, worst,
REFLECTION_COEF, test1);
resp1 = calc_error (test1);
/* test the reflection against the best vertex and expand it if the
reflection is better. if not, keep the reflection */
if (resp1 < response[best])
{
/* try expanding */
calc_reflection (simplex, centroid, worst, EXPANSION_COEF, test2);
resp2 = calc_error (test2);
if (resp2 <= resp1) /* keep expansion */
replace (simplex, response, worst, test2, resp2);
else /* keep reflection */
replace (simplex, response, worst, test1, resp1);
}
else if (resp1 < response[next])
{
/* new response is between the best and next worst
so keep reflection */
replace (simplex, response, worst, test1, resp1);
}
else
{
/* try contraction */
if (resp1 >= response[worst])
calc_reflection (simplex, centroid, worst,
-CONTRACTION_COEF, test2);
else
calc_reflection (simplex, centroid, worst,
CONTRACTION_COEF, test2);
resp2 = calc_error (test2);
/* test the contracted response against the worst response */
if (resp2 > response[worst])
{
/* new contracted response is worse, so decrease step size
and restart */
num_iter = 0;
num_restarts += 1;
restart (simplex, response, step_size);
}
else /* keep contraction */
replace (simplex, response, worst, test2, resp2);
}
/* test to determine when to stop.
first, check the number of iterations */
num_iter += 1; /* increment iteration counter */
if (num_iter >= MAX_ITERATIONS)
{
/* restart with smaller steps */
num_iter = 0;
num_restarts += 1;
restart (simplex, response, step_size);
}
/* limit the number of restarts */
if (num_restarts == MAX_RESTARTS) done = 1;
/* compare relative standard deviation of vertex responses
against a defined tolerance limit */
fit = calc_good_fit (response);
if (fit <= TOLERANCE) done = 1;
/* if done, copy the best solution to the output array */
if (done)
{
eval_vertices (response, &worst, &next, &best);
for (i = 0; i < DIMENSION; i++)
parameters[i] = simplex[best][i];
*sse = response[best];
}
} /* while (!done) */
number_restarts = num_restarts;
deallocate_arrays (&simplex, ¢roid, &response, &step_size,
&test1, &test2);
}
const Conserved SinkFlux::calcHydroFlux
(const Tessellation& tess,
const vector<Vector2D>& point_velocities,
const vector<ComputationalCell>& cells,
const EquationOfState& eos,
const size_t i) const
{
const Edge& edge = tess.GetEdge(static_cast<int>(i));
const std::pair<bool,bool> flags
(edge.neighbors.first>=0 && edge.neighbors.first<tess.GetPointNo(),
edge.neighbors.second>=0 && edge.neighbors.second<tess.GetPointNo());
assert(flags.first || flags.second);
if(!flags.first){
const size_t right_index =
static_cast<size_t>(edge.neighbors.second);
const ComputationalCell& right_cell = cells[right_index];
if(right_cell.stickers.find("dummy")->second)
return Conserved();
const Vector2D p = Parallel(edge);
const Primitive right = convert_to_primitive(right_cell,eos);
// const Primitive left = reflect(right,p);
const Primitive left = right;
const Vector2D n = remove_parallel_component
(tess.GetMeshPoint(edge.neighbors.second) -
edge.vertices.first, p);
return rotate_solve_rotate_back
(rs_, left, right, 0, n, p);
}
if(!flags.second){
const size_t left_index =
static_cast<size_t>(edge.neighbors.first);
const ComputationalCell& left_cell = cells[left_index];
if(left_cell.stickers.find("dummy")->second)
return Conserved();
const Primitive left = convert_to_primitive(left_cell, eos);
const Vector2D p = Parallel(edge);
// const Primitive right = reflect(left,p);
const Primitive right = left;
const Vector2D n = remove_parallel_component
(edge.vertices.second -
tess.GetMeshPoint(edge.neighbors.first), p);
return rotate_solve_rotate_back
(rs_, left, right, 0, n, p);
}
const size_t left_index =
static_cast<size_t>(edge.neighbors.first);
const size_t right_index =
static_cast<size_t>(edge.neighbors.second);
const ComputationalCell& left_cell = cells[left_index];
const ComputationalCell& right_cell = cells[right_index];
if(left_cell.stickers.find("dummy")->second &&
right_cell.stickers.find("dummy")->second)
return Conserved();
const Vector2D p = Parallel(edge);
const Vector2D n =
tess.GetMeshPoint(edge.neighbors.second) -
tess.GetMeshPoint(edge.neighbors.first);
const double velocity = Projection
(tess.CalcFaceVelocity
(point_velocities[left_index],
point_velocities[right_index],
tess.GetCellCM(edge.neighbors.first),
tess.GetCellCM(edge.neighbors.second),
calc_centroid(edge)),n);
if(left_cell.stickers.find("dummy")->second){
const Primitive right =
convert_to_primitive(right_cell, eos);
ComputationalCell ghost;
ghost.density = right.Density/100;
ghost.pressure = right.Pressure/100;
ghost.velocity = Vector2D(0,0);
const Primitive left = convert_to_primitive(ghost,eos);
/*
ScalarProd(n,right.Velocity) < 0 ? right :
reflect(right,p);
*/
return rotate_solve_rotate_back
(rs_,left,right,velocity,n,p);
}
if(right_cell.stickers.find("dummy")->second){
const Primitive left =
convert_to_primitive(left_cell, eos);
ComputationalCell ghost;
ghost.density = left.Density/100;
ghost.pressure = left.Pressure/100;
ghost.velocity = Vector2D(0,0);
const Primitive right = convert_to_primitive(ghost,eos);
/*
ScalarProd(n,left.Velocity)>0 ?
left : reflect(left,p);
*/
return rotate_solve_rotate_back
(rs_,left,right,velocity,n,p);
}
const Primitive left =
convert_to_primitive(left_cell, eos);
const Primitive right =
convert_to_primitive(right_cell, eos);
return rotate_solve_rotate_back
(rs_,left,right,velocity,n,p);
}
