// @brief The process equation is used to update the systems state using the process euquations of the system.
// @param sigma_point The sigma point representing a system state.
// @param deltaT The amount of time that has passed since the previous update, in seconds.
// @param measurement The reading from the rate gyroscope in rad/s used to update the orientation.
// @return The new estimated system state.
arma::vec::fixed<IMUModel::size> IMUModel::timeUpdate(const arma::vec::fixed<size>& state, double deltaT) {
arma::vec::fixed<IMUModel::size> newState;
newState = state;
//make a rotation quaternion
const double omega = arma::norm(state.rows(VX, VZ)) + 0.00000000001;
//Negate to compensate for some later mistake.
//deltaT has been negative for a while and has masked an incorrect hack below
const double theta = -omega*deltaT*0.5;
const double sinTheta = sin(theta);
const double cosTheta = cos(theta);
arma::vec vq({cosTheta,state(VX)*sinTheta/omega,state(VY)*sinTheta/omega,state(VZ)*sinTheta/omega});
//calculate quaternion multiplication
//TODO replace with quaternion class
arma::vec qcross = arma::cross( vq.rows(1,3), state.rows(QX,QZ) );
newState(QW) = vq(0)*state(QW) - arma::dot(vq.rows(1,3), state.rows(QX,QZ));
newState(QX) = vq(0)*state(QX) + state(QW)*vq(1) + qcross(0);
newState(QY) = vq(0)*state(QY) + state(QW)*vq(2) + qcross(1);
newState(QZ) = vq(0)*state(QZ) + state(QW)*vq(3) + qcross(2);
return newState;
}
atom::Hierarchy create_coarse_molecule_from_molecule(
const atom::Hierarchy &mh,int num_beads,
Model *mdl,
float bead_radius,
bool add_conn_restraint) {
IMP_NEW(IMP::statistics::internal::ParticlesDataPoints, ddp,
(core::get_leaves(mh)));
IMP::statistics::internal::VQClustering vq(ddp,num_beads);
vq.run();
multifit::DataPointsAssignment assignment(ddp,&vq);
atom::Selections sel;
algebra::Vector3Ds vecs;
for (int i=0;i<num_beads;i++){
IMP::statistics::internal::Array1DD xyz =
assignment.get_cluster_engine()->get_center(i);
vecs.push_back(algebra::Vector3D(xyz[0],xyz[1],xyz[2]));
}
//todo - mass should be a parameter
atom::Hierarchy ret_prot=create_molecule(vecs,bead_radius,3,mdl);
ParticlesTemp leaves=core::get_leaves(ret_prot);
for (ParticlesTemp::iterator it = leaves.begin();it != leaves.end();it++){
sel.push_back(atom::Selection(atom::Hierarchy(*it)));
}
if (add_conn_restraint){
int k=1;//todo - make this a parameter
Restraint *r = atom::create_connectivity_restraint(sel,k);
if (r != nullptr){
mdl->add_restraint(r);}
}
return ret_prot;
}
IMPMULTIFIT_BEGIN_NAMESPACE
void get_anchors_for_density(em::DensityMap *dmap, int number_of_means,
float density_threshold, std::string pdb_filename,
std::string cmm_filename, std::string seg_filename,
std::string txt_filename) {
dmap->set_was_used(true);
IMP_NEW(multifit::DensityDataPoints, ddp, (dmap, density_threshold));
IMP::statistics::internal::VQClustering vq(ddp, number_of_means);
ddp->set_was_used(true);
vq.run();
multifit::DataPointsAssignment assignment(ddp, &vq);
multifit::AnchorsData ad(assignment.get_centers(), *(assignment.get_edges()));
multifit::write_pdb(pdb_filename, assignment);
// also write cmm string into a file:
if (cmm_filename != "") {
multifit::write_cmm(cmm_filename, "anchor_graph", ad);
}
if (seg_filename != "") {
float apix = dmap->get_spacing();
multifit::write_segments_as_mrc(dmap, assignment, apix, apix,
density_threshold, seg_filename);
}
if (txt_filename != "") {
multifit::write_txt(txt_filename, ad);
}
}
void msvq(double *x, double *cb, const int l, int *cbsize, const int stage,
int *index)
{
int i, j;
double *p;
static double *xx = NULL;
static int size;
if (xx == NULL) {
xx = dgetmem(l);
size = l;
}
if (size > l) {
free(xx);
xx = dgetmem(l);
size = l;
}
movem(x, xx, sizeof(*x), l);
for (i = 0; i < stage; i++) {
index[i] = vq(xx, cb, l, cbsize[i]);
p = cb + index[i] * l;
for (j = 0; j < l; j++)
xx[j] -= p[j];
cb += cbsize[i] * l;
}
}
AppLockPrivate(AppLock *parent)
: _id(-1),
_myLock(false),
_otherLock(false),
_parent(parent)
{
if (_mobilizedDb.isNull()) {
XSqlQuery q("SELECT EXISTS(SELECT 1"
" FROM pg_class c"
" JOIN pg_namespace n ON (relnamespace = n.oid)"
" WHERE relname = 'lock'"
" AND nspname = 'xt') AS mobilized;");
if (q.first())
_mobilizedDb = q.value("mobilized");
else
(void)ErrorReporter::error(QtCriticalMsg,
qobject_cast<QWidget*>(parent->parent()),
parent->tr("Locking Error"),
q, __FILE__, __LINE__);
}
XSqlQuery vq("SELECT compareversion('9.2.0') <= 0 AS isNew;");
if (vq.first()) {
_actPidCol = vq.value("isNew").toBool() ? "pid" : "procpid";
} else {
(void)ErrorReporter::error(QtCriticalMsg,
qobject_cast<QWidget*>(parent->parent()),
parent->tr("Locking Error"),
vq, __FILE__, __LINE__);
}
updateLockStatus();
}
void get_segmentation(em::DensityMap *dmap, double apix,
double density_threshold, int num_means,
const std::string pdb_filename,
const std::string cmm_filename,
const std::string seg_filename,
const std::string txt_filename) {
IMP_LOG_VERBOSE("start setting trn_em" << std::endl);
IMP_NEW(DensityDataPoints, ddp, (dmap, density_threshold));
IMP_LOG_VERBOSE("initialize calculation of initial centers" << std::endl);
statistics::internal::VQClustering vq(ddp, num_means);
vq.run();
DataPointsAssignment assignment(ddp, &vq);
AnchorsData ad(assignment.get_centers(), *(assignment.get_edges()));
multifit::write_pdb(pdb_filename, assignment);
// also write cmm string into a file:
if (cmm_filename != "") {
write_cmm(cmm_filename, "anchor_graph", ad);
}
if (seg_filename != "") {
write_segments_as_mrc(dmap, assignment, apix, apix, density_threshold,
seg_filename);
}
if (txt_filename != "") {
write_txt(txt_filename, ad);
}
}
INT16 CGEN_IGNORE dlm_vq(FLOAT64* X, INT32 nRX, FLOAT64* Q, INT32 nCQ, INT32 nRQ, INT32* I) {
INT32 iRX = 0;
extern int vq(FLOAT64*, FLOAT64*, const INT32, const INT32);
for (iRX = 0; iRX < nRX; iRX++) {
I[iRX] = vq(X + iRX * nCQ, Q, nCQ, nRQ);
}
return O_K;
}
SE_Vector3f SE_Quat::map(const SE_Vector3f& v) const
{
SE_Quat vq(v, 0);
if(!vq.hasInverse())
{
return SE_Vector3f(0, 0, 0);
}
SE_Quat ret = mul(vq).mul(inverse());
return SE_Vector3f(ret.x, ret.y , ret.z);
}
T operator()(RawArray<const T,2> x) const {
// Temporary arrays and views
GEODE_ASSERT(x.sizes()==vec(n+3,d));
// Collect quadrature points
Array<T,2> tq(n,quads,uninit);
Array<T,3> xq(n,quads,d,uninit);
Array<T,3> vq(n,quads,d,uninit);
for (int i=0;i<n;i++) {
T_INFO(i)
for (int q=0;q<quads;q++) {
const T s = samples[q];
tq(i,q) = t1+dt*s;
SPLINE_INFO(s)
for (int a=0;a<d;a++) {
X_INFO(i,a)
xq(i,q,a) = a0*x0+a1*x1+a2*x2+a3*x3;
vq(i,q,a) = b0*x0+b1*x1+b2*x2+b3*x3;
}
}
}
// Compute energy
const auto Uq_ = U(tq.reshape_own(n*quads),NdArray<const T>(qshape,xq.flat),NdArray<const T>(qshape,vq.flat));
GEODE_ASSERT(Uq_.size()==n*quads);
const auto Uq = Uq_.reshape(n,quads);
// Accumulate
T sum = 0;
for (int i=0;i<n;i++) {
const T dt = t[i+2]-t[i+1];
for (int q=0;q<quads;q++)
sum += weights[q]*dt*Uq(i,q);
}
return sum;
}
// [[Rcpp::export]]
Rcpp::List worstcase_l1(Rcpp::NumericVector z, Rcpp::NumericVector q, double t){
// resulting probability
craam::numvec p;
// resulting objective value
double objective;
craam::numvec vz(z.begin(), z.end()), vq(q.begin(), q.end());
std::tie(p,objective) = craam::worstcase_l1(vz,vq,t);
Rcpp::List result;
result["p"] = Rcpp::NumericVector(p.cbegin(), p.cend());
result["obj"] = objective;
return result;
}
atom::Hierarchy create_coarse_molecule_from_density(em::DensityMap *dmap,
float dens_threshold,
int num_beads,
kernel::Model *mdl,
float bead_radius) {
IMP_NEW(DensityDataPoints, ddp, (dmap, dens_threshold));
IMP_LOG_VERBOSE("initialize calculation of initial centers" << std::endl);
IMP::statistics::internal::VQClustering vq(ddp, num_beads);
vq.run();
multifit::DataPointsAssignment assignment(ddp, &vq);
algebra::Vector3Ds vecs;
for (int i = 0; i < num_beads; i++) {
IMP::statistics::internal::Array1DD xyz =
assignment.get_cluster_engine()->get_center(i);
vecs.push_back(algebra::Vector3D(xyz[0], xyz[1], xyz[2]));
}
// todo - mass should be a parameter
atom::Hierarchy ret_prot = create_molecule(vecs, bead_radius, 3, mdl);
return ret_prot;
}
void SplitVQ(
float *qX, /* (o) the quantized vector */
int *index, /* (o) a vector of indexes for all vector
codebooks in the split */
float *X, /* (i) the vector to quantize */
const float *CB,/* (i) the quantizer codebook */
int nsplit, /* the number of vector splits */
const int *dim, /* the dimension of X and qX */
const int *cbsize /* the number of vectors in the codebook */
){
int cb_pos, X_pos, i;
cb_pos = 0;
X_pos= 0;
for (i = 0; i < nsplit; i++) {
vq(qX + X_pos, index + i, CB + cb_pos, X + X_pos,
cbsize[i], dim[i]);
X_pos += dim[i];
cb_pos += dim[i] * cbsize[i];
}
}
void
utils3D::rotateVecByQuat(Vector3& vec, const Vector3& axis, const float angle)
{
float mag = vec.magnitude();
vec.normalise();
Quaternion vq(0, vec.x, vec.y, vec.z);
Quaternion q;
// HAUKAP - put this in quaternion class as another constructor
Quaternion rot( cos(angle/2), axis.x * sin(angle/2), axis.y * sin(angle/2), axis.z * sin(angle/2) );
q = rot * q;
q.normalise();
Quaternion qp(q.r, -q.x, -q.y, -q.z);
Quaternion vp = qp * vq * q;
vec.x = vp.x * mag;
vec.y = vp.y * mag;
vec.z = vp.z * mag;
return;
}
/** solve one of the eight Appollonius Equations
| Cx - Ci|^2=(Rx+Ri)^2
with Cx the center of the common tangent circle, Rx the radius. Ci and Ri are the Center and radius of the i-th existing circle
**/
QList<RS_Circle> RS_Circle::solveAppolloniusSingle(const QList<RS_Circle>& circles)
{
// std::cout<<__FILE__<<" : "<<__FUNCTION__<<" : line "<<__LINE__<<std::endl;
// for(int i=0;i<circles.size();i++){
//std::cout<<"i="<<i<<"\t center="<<circles[i].getCenter()<<"\tr="<<circles[i].getRadius()<<std::endl;
// }
QList<RS_Circle> ret;
QList<RS_Vector> centers;
QList<double> radii;
for(size_t i=0;i<3;i++){
if(circles[i].getCenter().valid==false) return ret;
centers.push_back(circles[i].getCenter());
radii.push_back(fabs(circles[i].getRadius()));
}
/** form the linear equation to solve center in radius **/
QVector<QVector<double> > mat(2,QVector<double>(3,0.));
mat[0][0]=centers[2].x - centers[0].x;
mat[0][1]=centers[2].y - centers[0].y;
mat[1][0]=centers[2].x - centers[1].x;
mat[1][1]=centers[2].y - centers[1].y;
if(fabs(mat[0][0]*mat[1][1] - mat[0][1]*mat[1][0])<RS_TOLERANCE*RS_TOLERANCE){
// DEBUG_HEADER();
// std::cout<<"The provided circles are in a line, not common tangent circle"<<std::endl;
size_t i0=0;
if( centers[0].distanceTo(centers[1]) <= RS_TOLERANCE || centers[0].distanceTo(centers[2]) <= RS_TOLERANCE) i0 = 1;
LC_Quadratic lc0(& (circles[i0]), & (circles[(i0+1)%3]));
LC_Quadratic lc1(& (circles[i0]), & (circles[(i0+2)%3]));
auto&& c0 = LC_Quadratic::getIntersection(lc0, lc1);
// qDebug()<<"c0.size()="<<c0.size();
for(size_t i=0; i<c0.size(); i++){
const double dc = c0[i].distanceTo(centers[i0]);
ret<<RS_Circle(NULL, RS_CircleData(c0[i], fabs(dc - radii[i0])));
if( dc > radii[i0]) {
ret<<RS_Circle(NULL, RS_CircleData(c0[i], dc + radii[i0]));
}
}
return ret;
}
// r^0 term
mat[0][2]=0.5*(centers[2].squared()-centers[0].squared()+radii[0]*radii[0]-radii[2]*radii[2]);
mat[1][2]=0.5*(centers[2].squared()-centers[1].squared()+radii[1]*radii[1]-radii[2]*radii[2]);
std::cout<<__FILE__<<" : "<<__FUNCTION__<<" : line "<<__LINE__<<std::endl;
for(unsigned short i=0;i<=1;i++){
std::cout<<"eqs P:"<<i<<" : "<<mat[i][0]<<"*x + "<<mat[i][1]<<"*y = "<<mat[i][2]<<std::endl;
}
// QVector<QVector<double> > sm(2,QVector<double>(2,0.));
QVector<double> sm(2,0.);
if(RS_Math::linearSolver(mat,sm)==false){
return ret;
}
RS_Vector vp(sm[0],sm[1]);
// std::cout<<__FILE__<<" : "<<__FUNCTION__<<" : line "<<__LINE__<<std::endl;
// std::cout<<"vp="<<vp<<std::endl;
// r term
mat[0][2]= radii[0]-radii[2];
mat[1][2]= radii[1]-radii[2];
// for(unsigned short i=0;i<=1;i++){
// std::cout<<"eqs Q:"<<i<<" : "<<mat[i][0]<<"*x + "<<mat[i][1]<<"*y = "<<mat[i][2]<<std::endl;
// }
if(RS_Math::linearSolver(mat,sm)==false){
return ret;
}
RS_Vector vq(sm[0],sm[1]);
// std::cout<<"vq="<<vq<<std::endl;
//form quadratic equation for r
RS_Vector dcp=vp-centers[0];
double a=vq.squared()-1.;
if(fabs(a)<RS_TOLERANCE*1e-4) {
return ret;
}
std::vector<double> ce(0,0.);
ce.push_back(2.*(dcp.dotP(vq)-radii[0])/a);
ce.push_back((dcp.squared()-radii[0]*radii[0])/a);
std::vector<double>&& vr=RS_Math::quadraticSolver(ce);
for(size_t i=0; i < vr.size();i++){
if(vr.at(i)<RS_TOLERANCE) continue;
ret<<RS_Circle(NULL,RS_CircleData(vp+vq*vr.at(i),vr.at(i)));
}
// std::cout<<__FILE__<<" : "<<__FUNCTION__<<" : line "<<__LINE__<<std::endl;
// std::cout<<"Found "<<ret.size()<<" solutions"<<std::endl;
return ret;
}
double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
{
btVector3 vp(p1[0], p1[1], p1[2]);
btTriangleShape trishapeA(vp,
btVector3(p2[0], p2[1], p2[2]),
btVector3(p3[0], p3[1], p3[2]));
trishapeA.setMargin(0.000001f);
btVector3 vq(q1[0], q1[1], q1[2]);
btTriangleShape trishapeB(vq,
btVector3(q2[0], q2[1], q2[2]),
btVector3(q3[0], q3[1], q3[2]));
trishapeB.setMargin(0.000001f);
// btVoronoiSimplexSolver sGjkSimplexSolver;
// btGjkEpaPenetrationDepthSolver penSolverPtr;
static btSimplexSolverInterface sGjkSimplexSolver;
sGjkSimplexSolver.reset();
static btGjkEpaPenetrationDepthSolver Solver0;
static btMinkowskiPenetrationDepthSolver Solver1;
btConvexPenetrationDepthSolver* Solver = NULL;
Solver = &Solver1;
btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
convexConvex.m_catchDegeneracies = 1;
// btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
btTransform tr;
tr.setIdentity();
input.m_transformA = tr;
input.m_transformB = tr;
convexConvex.getClosestPoints(input, gjkOutput, 0);
if (gjkOutput.m_hasResult)
{
pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
normal[0] = gjkOutput.m_normalOnBInWorld[0];
normal[1] = gjkOutput.m_normalOnBInWorld[1];
normal[2] = gjkOutput.m_normalOnBInWorld[2];
return gjkOutput.m_distance;
}
return -1.0f;
}
/*
* Read mean and variance S3-format files and cluster them as follows:
* assume single feature vector
* for each dimension of feature vector {
* create list of <mean,var> pairs in the entire codebook set (for that dimension);
* // HACK!! 0 vectors omitted from the above list
* VQ cluster this list into the given number of vqsize points;
* replace codebook entries with nearest clustered values (for that dimension);
* // HACK!! 0 vectors remain untouched
* }
* write remapped codebooks to files "mean-vq" and "var-vq";
*/
main (int32 argc, char *argv[])
{
float32 ****mean, ****var;
int32 n_cb, n_feat, n_den, *featlen;
int32 i, j, c, w, dim, p, n_pt, vqsize;
float32 **pt, **cb;
float64 err;
FILE *fp;
if ((argc < 4) || (sscanf (argv[3], "%d", &vqsize) != 1))
E_FATAL("Usage: %s meanfile varfile vqsize\n", argv[0]);
if (argc > 4) {
if ((fp = fopen(argv[4], "w")) != NULL) {
*stdout = *fp;
*stderr = *fp;
} else
E_ERROR("fopen(%s,w) failed\n", argv[4]);
}
gauden_param_read (&mean, &n_cb, &n_feat, &n_den, &featlen, argv[1]);
E_INFO("%s: %d x %d x %d ", argv[1], n_cb, n_feat, n_den);
for (i = 0; i < n_feat; i++)
printf (" %d", featlen[i]);
printf ("\n");
gauden_param_read (&var, &n_cb, &n_feat, &n_den, &featlen, argv[2]);
E_INFO("%s: %d x %d x %d ", argv[2], n_cb, n_feat, n_den);
for (i = 0; i < n_feat; i++)
printf (" %d", featlen[i]);
printf ("\n");
assert (n_feat == 1);
n_pt = n_cb * n_den;
pt = (float32 **) ckd_calloc_2d (n_pt, 2, sizeof(float32));
cb = (float32 **) ckd_calloc_2d (vqsize, 2, sizeof(float32));
tmg = timing_new ();
/* VQ each column of mean,var pair (assuming just one feature vector) */
for (dim = 0; dim < featlen[0]; dim++) {
j = 0;
for (c = 0; c < n_cb; c++) {
for (w = 0; w < n_den; w++) {
if ((mean[c][0][w][dim] != 0.0) || (var[c][0][w][dim] != 0.0)) {
pt[j][0] = mean[c][0][w][dim];
pt[j][1] = var[c][0][w][dim];
j++;
}
}
}
err = vq (pt, cb, j, vqsize, 2);
vq_dump (cb, vqsize, 2);
j = 0;
for (c = 0; c < n_cb; c++) {
for (w = 0; w < n_den; w++) {
if ((mean[c][0][w][dim] != 0.0) || (var[c][0][w][dim] != 0.0)) {
mean[c][0][w][dim] = pt[j][0];
var[c][0][w][dim] = pt[j][1];
j++;
}
}
}
E_INFO("%d values quantized for dimension %d; error = %e\n", j, dim, err);
}
fflush (fp);
gauden_param_write (mean, n_cb, n_feat, n_den, featlen, "mean-vq");
gauden_param_write (var, n_cb, n_feat, n_den, featlen, "var-vq");
fflush (fp);
exit(0);
}
vec3& transform( vec3& out, const quaternion& q, const vec3& v )
{
quaternion vq(v[0], v[1], v[2], 0.0f);
out = ( q * vq * conj(q) ).comps().xyz();
return out;
}
int main(int argc, char **argv)
{
FILE *fp = stdin;
GMM gmm, tgmm, floor;
double E = DEF_E, V = DEF_V, W = DEF_W,
*dat, *pd, *cb, *icb, *logwgd, logb, *sum, *sum_m, **sum_v, diff, sum_w,
ave_logp0, ave_logp1, change = MAXVALUE, tmp1, tmp2;
int ispipe, l, ll, L = DEF_L, m, M = DEF_M, N, t, T = DEF_T, S =
DEF_S, full = FULL, n1, i, j, Imin = DEF_IMIN, Imax =
DEF_IMAX, *tindex, *cntcb;
if ((cmnd = strrchr(argv[0], '/')) == NULL)
cmnd = argv[0];
else
cmnd++;
/* -- Check options -- */
while (--argc)
if (**++argv == '-') {
switch (*(*argv + 1)) {
case 'h':
usage(0);
break;
case 'l':
L = atoi(*++argv);
--argc;
break;
case 'm':
M = atoi(*++argv);
--argc;
break;
case 't':
T = atoi(*++argv);
--argc;
break;
case 's':
S = atoi(*++argv);
--argc;
break;
case 'a':
Imin = atoi(*++argv);
--argc;
break;
case 'b':
Imax = atoi(*++argv);
--argc;
break;
case 'e':
E = atof(*++argv);
--argc;
break;
case 'v':
V = atof(*++argv);
--argc;
break;
case 'w':
W = atof(*++argv);
--argc;
break;
case 'f':
full = 1 - full;
break;
default:
fprintf(stderr, "%s: Illegal option \"%s\".\n", cmnd, *argv);
usage(1);
}
} else
fp = getfp(*argv, "rb");
/* -- Count number of training vectors -- */
if (T == -1) {
ispipe = fseek(fp, 0L, SEEK_END);
T = (int) (ftell(fp) / (double) L / (double) sizeof(float));
rewind(fp);
if (ispipe == -1) { /* training data is from standard input via pipe */
fprintf(stderr,
"\n %s (Error) -t option must be specified for the standard input via pipe.\n",
cmnd);
usage(1);
}
}
/* Memory allocation */
/* Training data */
dat = dgetmem(T * L);
/* for VQ */
N = 1;
while (N < M)
N *= 2;
cb = dgetmem(N * L);
icb = dgetmem(L);
tindex = (int *) getmem(T, sizeof(int));
cntcb = (int *) getmem(M, sizeof(int));
/* GMM */
gmm.weight = dgetmem(M);
gmm.gauss = (Gauss *) getmem(M, sizeof(Gauss));
for (m = 0; m < M; m++) {
gmm.gauss[m].mean = dgetmem(L);
gmm.gauss[m].var = dgetmem(L);
if (full == 1) {
gmm.gauss[m].cov = (double **) malloc(sizeof(double *) * L);
gmm.gauss[m].inv = (double **) malloc(sizeof(double *) * L);
for (l = 0; l < L; l++) {
gmm.gauss[m].cov[l] = dgetmem(L);
gmm.gauss[m].inv[l] = dgetmem(L);
}
}
}
if (full == 1) {
floor.gauss = (Gauss *) getmem(1, sizeof(Gauss));
floor.gauss[0].cov = (double **) malloc(sizeof(double *) * L);
for (l = 0; l < L; l++)
floor.gauss[0].cov[l] = dgetmem(L);
sum_m = dgetmem(L);
sum_v = (double **) malloc(sizeof(double *) * L);
}
/* temporary */
tgmm.weight = dgetmem(M);
tgmm.gauss = (Gauss *) getmem(M, sizeof(Gauss));
for (m = 0; m < M; m++) {
tgmm.gauss[m].mean = dgetmem(L);
tgmm.gauss[m].var = dgetmem(L);
if (full == 1) {
tgmm.gauss[m].cov = (double **) malloc(sizeof(double *) * L);
tgmm.gauss[m].inv = (double **) malloc(sizeof(double *) * L);
for (l = 0; l < L; l++) {
tgmm.gauss[m].cov[l] = dgetmem(L);
tgmm.gauss[m].inv[l] = dgetmem(L);
}
}
}
logwgd = dgetmem(M);
sum = dgetmem(M);
/* Read training data */
freadf(dat, sizeof(*dat), T * L, fp);
/* Initialization of GMM parameters */
/* LBG */
vaverage(dat, L, T, icb);
lbg(dat, L, T, icb, 1, cb, N, ITER, MINTRAIN, S, CENTUP, DELTA, END);
for (t = 0, pd = dat; t < T; t++, pd += L) {
tindex[t] = vq(pd, cb, L, M);
cntcb[tindex[t]]++;
}
for (m = 0; m < M; m++)
if (cntcb[m] == 0) {
fprintf(stderr, "Error: No data for mixture No.%d\n", m);
usage(1);
}
fprintf(stderr, "T = %d L = %d M = %d\n", T, L, M);
/* flooring value for weights */
W = 1.0 / (double) M *(double) W;
/* weights */
for (m = 0, sum_w = 0.0; m < M; m++) {
gmm.weight[m] = (double) cntcb[m] / (double) T;
if (gmm.weight[m] < W)
gmm.weight[m] = W;
sum_w += gmm.weight[m];
}
if (sum_w != 1.0)
for (m = 0; m < M; m++)
gmm.weight[m] /= sum_w;
/* mean */
for (m = 0, pd = cb; m < M; m++, pd += L)
movem(pd, gmm.gauss[m].mean, sizeof(double), L);
/* variance */
if (full != 1) {
for (t = 0, pd = dat; t < T; t++, pd += L)
for (l = 0; l < L; l++) {
diff = gmm.gauss[tindex[t]].mean[l] - pd[l];
gmm.gauss[tindex[t]].var[l] += sq(diff);
}
for (m = 0; m < M; m++)
for (l = 0; l < L; l++) {
gmm.gauss[m].var[l] /= (double) cntcb[m];
if (gmm.gauss[m].var[l] < V)
gmm.gauss[m].var[l] = V;
}
for (m = 0; m < M; m++)
gmm.gauss[m].gconst = cal_gconst(gmm.gauss[m].var, L);
}
/* full covariance */
else {
for (t = 0, pd = dat; t < T; t++, pd += L) {
for (l = 0; l < L; l++) {
for (i = 0; i <= l; i++) {
if (l == i) {
diff =
(gmm.gauss[tindex[t]].mean[l] -
pd[l]) * (gmm.gauss[tindex[t]].mean[i] - pd[i]);
floor.gauss[0].cov[l][i] += diff;
}
}
}
}
for (l = 0; l < L; l++) {
for (i = 0; i <= l; i++) {
if (l == i) {
floor.gauss[0].cov[l][i] /= T;
floor.gauss[0].cov[l][i] *= V;
}
}
}
for (t = 0, pd = dat; t < T; t++, pd += L) {
for (l = 0; l < L; l++) {
for (i = 0; i <= l; i++) {
diff =
(gmm.gauss[tindex[t]].mean[l] -
pd[l]) * (gmm.gauss[tindex[t]].mean[i] - pd[i]);
gmm.gauss[tindex[t]].cov[l][i] += diff;
}
}
}
for (m = 0; m < M; m++)
for (l = 0; l < L; l++)
for (i = 0; i <= l; i++) {
gmm.gauss[m].cov[l][i] /= (double) cntcb[m];
}
}
/* EM training of GMM parameters */
for (i = 0; (i <= Imax) && ((i <= Imin) || (fabs(change) > E)); i++) {
if (full != 1)
fillz_gmm(&tgmm, M, L);
else
fillz_gmmf(&tgmm, M, L);
fillz(sum, sizeof(double), M);
if (full != 1) {
for (m = 0; m < M; m++)
gmm.gauss[m].gconst = cal_gconst(gmm.gauss[m].var, L);
} else {
for (m = 0, n1 = 0; m < M; m++) {
gmm.gauss[m].gconst = cal_gconstf(gmm.gauss[m].cov, L);
if (gmm.gauss[m].gconst == 0) {
n1++;
for (l = 0; l < L; l++)
gmm.gauss[m].cov[l][l] += floor.gauss[0].cov[l][l];
gmm.gauss[m].gconst = cal_gconstf(gmm.gauss[m].cov, L);
}
if (gmm.gauss[m].gconst == 0) {
fprintf(stderr, "ERROR : Can't caluculate covdet");
exit(EXIT_FAILURE);
}
/* calculate inv */
cal_inv(gmm.gauss[m].cov, gmm.gauss[m].inv, L);
}
}
if (full == 1)
fprintf(stderr, "%d cov can't caluculate covdet\n", n1);
for (t = 0, ave_logp1 = 0.0, pd = dat; t < T; t++, pd += L) {
for (m = 0, logb = LZERO; m < M; m++) {
if (full != 1) {
logwgd[m] = log_wgd(&gmm, m, pd, L);
logb = log_add(logb, logwgd[m]);
}
/* full */
else {
logwgd[m] = log_wgdf(&gmm, m, pd, L);
logb = log_add(logb, logwgd[m]);
}
}
ave_logp1 += logb;
for (m = 0; m < M; m++) {
tmp1 = exp(logwgd[m] - logb);
sum[m] += tmp1;
for (l = 0; l < L; l++) {
tmp2 = tmp1 * pd[l];
tgmm.gauss[m].mean[l] += tmp2;
if (full != 1)
tgmm.gauss[m].var[l] += tmp2 * pd[l];
else {
for (j = 0; j <= l; j++) {
tgmm.gauss[m].cov[l][j] +=
tmp1 * (pd[l] - gmm.gauss[m].mean[l]) * (pd[j] -
gmm.
gauss[m].mean
[j]);
}
}
}
}
}
/* Output average log likelihood at each iteration */
ave_logp1 /= (double) T;
if (i == 1 && m == 1)
ave_logp0 = ave_logp1;
fprintf(stderr, "iter %3d : ", i);
fprintf(stderr, "ave_logprob = %g", ave_logp1);
if (i) {
change = ave_logp1 - ave_logp0;
fprintf(stderr, " change = %g", change);
}
fprintf(stderr, "\n");
ave_logp0 = ave_logp1;
/* Update perameters */
/* weights */
for (m = 0; m < M; m++)
gmm.weight[m] = sum[m] / (double) T;
/* mean, variance */
for (m = 0; m < M; m++) {
for (l = 0; l < L; l++)
gmm.gauss[m].mean[l] = tgmm.gauss[m].mean[l] / sum[m];
if (full != 1) {
for (l = 0; l < L; l++) {
gmm.gauss[m].var[l] =
tgmm.gauss[m].var[l] / sum[m] - sq(gmm.gauss[m].mean[l]);
if (gmm.gauss[m].var[l] < V)
gmm.gauss[m].var[l] = V;
}
}
/* full */
else {
for (l = 0; l < L; l++) {
for (j = 0; j <= l; j++) {
gmm.gauss[m].cov[l][j] = tgmm.gauss[m].cov[l][j] / sum[m];
}
}
}
}
}
/* Output GMM parameters */
fwritef(gmm.weight, sizeof(double), M, stdout);
if (full != 1) {
for (m = 0; m < M; m++) {
fwritef(gmm.gauss[m].mean, sizeof(double), L, stdout);
fwritef(gmm.gauss[m].var, sizeof(double), L, stdout);
}
} else {
for (m = 0; m < M; m++) {
fwritef(gmm.gauss[m].mean, sizeof(double), L, stdout);
for (i = 0; i < L; i++)
for (j = 0; j < i; j++)
gmm.gauss[m].cov[j][i] = gmm.gauss[m].cov[i][j];
for (l = 0; l < L; l++)
fwritef(gmm.gauss[m].cov[l], sizeof(double), L, stdout);
}
}
return (0);
}
int main()
{
VirtioDriver vio(VIRTIO_0_BASE);
vio.init();
kout << "Magic value is " << Hex(vio.reg32<VirtioDriver::V2_MagicValue>()) << "\n";
kout << "Version = " << vio.reg32<VirtioDriver::V2_Version>() << "\n";
kout << "VendorID = " << Hex(vio.reg32<VirtioDriver::V2_VendorID>()) << "\n";
kout << "DeviceID = " << Hex(vio.reg32<VirtioDriver::V2_DeviceID>()) << "\n";
kout << "HostFeatures = " << Hex(vio.reg32<VirtioDriver::V1_HostFeatures>()) << "\n";
kout << "GuestPageSize = " << Hex(vio.reg32<VirtioDriver::V1_GuestPageSize>()) << "\n";
kout << "DeviceStatus = " << Hex(vio.reg32<VirtioDriver::V1_Status>()) << "\n";
kout << "QueueNumMax = " << vio.reg32<VirtioDriver::V1_QueueNumMax>() << "\n";
auto blkConf = vio.regcopy<VirtioBlockConfig,VirtioDriver::V1_Config>();
blkConf.dump();
// return 0;
// 至少包含2个对齐的PAGE
char queueBuffer[VirtioDriver::PAGE_SIZE_4KB*3];
// 对齐处的地址
uint64_t pageAddr = alignAhead(reinterpret_cast<uint64_t>(queueBuffer),VirtioDriver::PAGE_SIZE_4KB);
// legacy需要3个，[0]=8byte,3fields, [1]=data,[2]=1byte status
size_t descrNum = 4;
uint16_t startDescrIndex=1;
constexpr size_t readBytes =
// 3
// 512*3-1 // invalid
512*3 // 必须是512的n倍
;
// qsize
// mem_size
//
VirtioQueueLayout vq(reinterpret_cast<void*>(pageAddr),descrNum,VirtioDriver::USED_RING_ALIGNMENT);
auto memSize = vq.memSize();
kout << "virtqueue memSize = " << memSize << "\n";
// 应当清0
std::memset(vq.queueBase(), 0, memSize);
// reqeust
size_t blkNum = 1;
size_t reqBufSize = VirtioBlockRequestRef::memSize(blkNum);
if(reqBufSize >= readBytes + 512)
{
kout << FATAL << "req size overflows\n";
return 1;
}
// 至少包含readBytes个
char reqBuf[readBytes+512];
VirtioBlockRequestRef vreq(reqBuf,blkNum,VirtioBlockRequestRef::REQ_IN,0);
const char * invalidStr = "if you see this, then it is invalid\n";
std::memcpy(vreq.data(),invalidStr,std::strlen(invalidStr));
VirtioQueueDescriptor * descs[3];
vq.allocateDescriptros(descs, 3);
vq.chainDescriptors(descs, 3);
// descs[0]
// DW = DeviceWriteOnly,DR = DeviceReadOnly
// [0]=header,DR,[1]=status,DW[2]=empty buffer,DW
// or
// [0] = header + status + empty buffer, DW
// 前一个用于legacy模式
auto descTable = vq.descTable();
new (&descTable[startDescrIndex]) VirtioQueueDescriptor(reinterpret_cast<uint64_t>(vreq.base()),16,bitOnes<VirtioQueueDescriptor::VIRTQ_DESC_F_NEXT>(),startDescrIndex+1);
new (&descTable[startDescrIndex+1]) VirtioQueueDescriptor(descTable[startDescrIndex+1].addr() + 16,readBytes,bitOnes<VirtioQueueDescriptor::VIRTQ_DESC_F_NEXT,VirtioQueueDescriptor::VIRTQ_DESC_F_WRITE>(),startDescrIndex+2);
new (&descTable[startDescrIndex+2]) VirtioQueueDescriptor(descTable[startDescrIndex+2].addr() + readBytes,1,bitOnes<VirtioQueueDescriptor::VIRTQ_DESC_F_WRITE>(),0);
// 注意, old_idx和new_idx之间的差值就是本次请求的数目
auto & avRef = vq.availRing();
avRef.idx(1); //
avRef.ring()[0]=startDescrIndex;// header
// avRef.ring()[1]=startDescrIndex + 1;
/*
下面这些寄存器只写
case VIRTIO_MMIO_DEVICE_FEATURES_SEL:
case VIRTIO_MMIO_DRIVER_FEATURES:
case VIRTIO_MMIO_DRIVER_FEATURES_SEL:
case VIRTIO_MMIO_GUEST_PAGE_SIZE:
case VIRTIO_MMIO_QUEUE_SEL:
case VIRTIO_MMIO_QUEUE_NUM:
case VIRTIO_MMIO_QUEUE_ALIGN:
case VIRTIO_MMIO_QUEUE_NOTIFY:
case VIRTIO_MMIO_INTERRUPT_ACK:
// 下面这些寄存器只读
case VIRTIO_MMIO_MAGIC_VALUE:
case VIRTIO_MMIO_VERSION:
case VIRTIO_MMIO_DEVICE_ID:
case VIRTIO_MMIO_VENDOR_ID:
case VIRTIO_MMIO_DEVICE_FEATURES:
case VIRTIO_MMIO_QUEUE_NUM_MAX:
case VIRTIO_MMIO_INTERRUPT_STATUS:
*/
/*
*
The virtual queue is configured as follows:
1. Select the queue writing its index (first queue is 0) to QueueSel.
2. Check if the queue is not already in use: read QueuePFN, expecting a returned value of zero (0x0).
3. Read maximum queue size (number of elements) from QueueNumMax. If the returned value is zero
(0x0) the queue is not available.
4. Allocate and zero the queue pages in contiguous virtual memory, aligning the Used Ring to an optimal boundary (usually page size). The driver should choose a queue size smaller than or equal to
QueueNumMax.
5. Notify the device about the queue size by writing the size to QueueNum.
6. Notify the device about the used alignment by writing its value in bytes to QueueAlign.
7. Write the physical number of the first page of the queue to the QueuePFN register.
*
*
*/
vio.reg32<VirtioDriver::V1_QueueSel>() = 0; // set current
vio.reg32<VirtioDriver::V1_QueueNum>()=descrNum; // set current queue size
vio.reg32<VirtioDriver::V1_QueuePFN>() = pageAddr / VirtioDriver::PAGE_SIZE_4KB; // 设置PFN
// notify index 0 completed
vio.reg32<VirtioDriver::V1_QueueNotify>()=0; // which queue?
// wait to complete
delayCPU(1000*1000);
kout << "=====read data is \n";
kout.print(reinterpret_cast<const char*>(vreq.data()),readBytes);
kout << "\n=================\n";
/*
Historically, many drivers ignored the len value, as a result, many devices set len incorrectly. Thus, when
using the legacy interface, it is generally a good idea to ignore the len value in used ring entries if possible.
Specific known issues are listed per device type.
对于block device,
When using the legacy interface, transitional drivers SHOULD ignore the len value in used ring entries.
Note: Historically, some devices put the total descriptor length, or the total length of device-writable buffers
there, even when only the status byte was actually written.
也就是说，legacy 模式下的len应当忽略
在实践中，len返回的总是readBytes + 1,因此应当忽略。
*/
// 读取usedRing
// 同样,old_idx和new_idx之间的差值也体现了本次完成的数目
auto &usedRingRef = vq.usedRing();
kout << "used ring idx = " << usedRingRef.idx() << "\n";
kout << "ring[0]{id,len} = " << usedRingRef.ring()[0]._id << "," << usedRingRef.ring()[0]._len<< "\n";
// kout << "ring[1]{id,len} = " << usedRingRef.ring()[1]._id << "," << usedRingRef.ring()[1]._len<< "\n";
return 0;
}
void main(int argc, char **argv)
{
int l = LENG, cbsize = CBSIZE, index;
Boolean qflag = QFLAG;
FILE *fp = stdin, *fpcb = NULL;
double *x, *qx, *cb;
if ((cmnd = strrchr(argv[0], '/')) == NULL)
cmnd = argv[0];
else
cmnd++;
while (--argc)
if (**++argv == '-'){
switch (*(*argv+1)) {
case 'l':
l = atoi(*++argv);
--argc;
break;
case 'n':
l = atoi(*++argv)+1;
--argc;
break;
case 's':
cbsize = atoi(*++argv);
--argc;
break;
case 'q':
qflag = 1 - qflag;
break;
case 'h':
usage(0);
default:
fprintf(stderr, "%s : Invalid option '%c' !\n", cmnd, *(*argv+1));
usage(1);
}
}
else if (fpcb == NULL)
fpcb = getfp(*argv, "r");
else
fp = getfp(*argv, "r");
fseek(fpcb,0,2);
#ifdef DOUBLE
cbsize = ftell(fpcb)/sizeof(double)/l;
#else
cbsize = ftell(fpcb)/sizeof(float)/l;
#endif
rewind(fpcb);
x = dgetmem(l+l+cbsize*l);
qx = x + l;
cb = qx + l;
if(freadf(cb, sizeof(*cb), cbsize*l, fpcb) != cbsize*l){
fprintf(stderr,"%s : Codebook size error !\n",cmnd);
exit(1);
}
if(! qflag)
while (freadf(x, sizeof(*x), l, fp) == l){
index = vq(x, cb, l, cbsize);
fwrite(&index, sizeof(index), 1, stdout);
}
else
while (freadf(x, sizeof(*x), l, fp) == l){
index = vq(x, cb, l, cbsize);
ivq(index, cb, l, qx);
fwritef(qx, sizeof(*qx), l, stdout);
}
exit(0);
}
void gradient(RawArray<const T,2> x, RawArray<T,2> grad) const {
// Temporary arrays and views
GEODE_ASSERT(x.sizes()==vec(n+3,d) && grad.sizes()==x.sizes());
const auto sx = smallx.flat.raw(),
sv = smallv.flat.raw();
// Collect quadrature points
const int e = 4*d;
Array<T,3> tq( n,quads,e,uninit);
Array<T,4> xq(vec(n,quads,e,d),uninit);
Array<T,4> vq(vec(n,quads,e,d),uninit);
for (int i=0;i<n;i++) {
T_INFO(i)
for (int q=0;q<quads;q++) {
const T s = samples[q],
t = t1+dt*s;
for (int j=0;j<e;j++)
tq(i,q,j) = t;
SPLINE_INFO(s)
for (int a=0;a<d;a++) {
X_INFO(i,a)
const T x = a0*x0+a1*x1+a2*x2+a3*x3,
v = b0*x0+b1*x1+b2*x2+b3*x3;
for (int j=0;j<e;j++) {
xq(i,q,j,a) = x;
vq(i,q,j,a) = v;
}
}
for (int a=0;a<d;a++) {
xq(i,q,4*a ,a) -= sx[a];
xq(i,q,4*a+1,a) += sx[a];
vq(i,q,4*a+2,a) -= sv[a];
vq(i,q,4*a+3,a) += sv[a];
}
}
}
// Compute energies
const auto Uq_ = U(tq.reshape_own(n*quads*e),NdArray<const T>(q2shape,xq.flat),NdArray<const T>(q2shape,vq.flat));
GEODE_ASSERT(Uq_.size()==n*quads*e);
const auto Uq = Uq_.reshape(n,quads,e);
// Accumulate
grad.fill(0);
const auto inv_2s = GEODE_RAW_ALLOCA(d,Vector<T,2>);
for (int a=0;a<d;a++)
inv_2s[a] = vec(.5/sx[a],.5/sv[a]);
for (int i=0;i<n;i++) {
T_INFO(i)
for (int q=0;q<quads;q++) {
const T s = samples[q],
w = dt*weights[q];
SPLINE_INFO(s)
for (int a=0;a<d;a++) {
const T wx = w*inv_2s[a].x*(Uq(i,q,4*a+1)-Uq(i,q,4*a )),
wv = w*inv_2s[a].y*(Uq(i,q,4*a+3)-Uq(i,q,4*a+2));
grad(i ,a) += a0*wx+b0*wv;
grad(i+1,a) += a1*wx+b1*wv;
grad(i+2,a) += a2*wx+b2*wv;
grad(i+3,a) += a3*wx+b3*wv;
}
}
}
}
void lbg(double *x, const int l, const int tnum, double *icb, int icbsize,
double *cb, const int ecbsize, const int iter, const int mintnum,
const int seed, const int centup, const double delta, const double end)
{
int i, j, k, it, maxindex, tnum1, tnum2;
static int *cntcb, *tindex, size, sizex, sizecb;
unsigned long next = SEED;
double d0, d1, dl, err, tmp, rand;
static double *cb1 = NULL;
double *p, *q, *r;
if (cb1 == NULL) {
cb1 = dgetmem(ecbsize * l);
tindex = (int *) dgetmem(tnum);
cntcb = (int *) dgetmem(ecbsize);
size = l;
sizex = tnum;
sizecb = ecbsize;
}
if (l > size) {
free(cb1);
cb1 = dgetmem(ecbsize * l);
size = l;
}
if (tnum > sizex) {
free(tindex);
tindex = (int *) dgetmem(tnum);
sizex = tnum;
}
if (sizecb > ecbsize) {
free(cb1);
free(cntcb);
cb1 = dgetmem(ecbsize * l);
cntcb = (int *) dgetmem(ecbsize);
}
movem(icb, cb, sizeof(*icb), icbsize * l);
if (seed != 1)
next = srnd((unsigned int) seed);
for (; icbsize * 2 <= ecbsize;) {
q = cb;
r = cb + icbsize * l;
for (i = 0; i < icbsize; i++) {
for (j = 0; j < l; j++) {
dl = delta * nrandom(&next);
*r = *q - dl;
r++;
*q = *q + dl;
q++;
}
}
icbsize *= 2;
d0 = MAXVALUE;
for (it = 1; it <= iter; it++) {
fillz((double *) cntcb, sizeof(*cntcb), icbsize);
d1 = 0.0;
p = x;
for (i = 0; i < tnum; i++, p += l) {
tindex[i] = vq(p, cb, l, icbsize);
cntcb[tindex[i]]++;
q = cb + tindex[i] * l;
d1 += edist(p, q, l);
}
d1 /= tnum;
err = abs((d0 - d1) / d1);
if (err < end)
break;
d0 = d1;
fillz(cb1, sizeof(*cb), icbsize * l);
p = x;
for (i = 0; i < tnum; i++) {
q = cb1 + tindex[i] * l;
for (j = 0; j < l; j++)
*q++ += *p++;
}
k = maxindex = 0;
for (i = 0; i < icbsize; i++)
if (cntcb[i] > k) {
k = cntcb[i];
maxindex = i;
}
q = cb;
r = cb1;
for (i = 0; i < icbsize; i++, r += l, q += l)
if (cntcb[i] >= mintnum)
for (j = 0; j < l; j++)
q[j] = r[j] / (double) cntcb[i];
else {
if (centup == 1) {
p = cb + maxindex * l;
for (j = 0; j < l; j++) {
rand = nrandom(&next);
q[j] = p[j] + delta * rand;
p[j] = p[j] - delta * rand;
}
} else if (centup == 2) {
if (i < icbsize / 2) {
p = q + icbsize / 2 * l;
tnum1 = cntcb[i];
tnum2 = cntcb[i + icbsize / 2];
for (j = 0; j < l; j++) {
tmp = (tnum2 * q[j] + tnum1 * p[j]) / (tnum1 + tnum2);
rand = nrandom(&next);
q[j] = tmp + delta * rand;
p[j] = tmp - delta * rand;
}
} else {
p = q - icbsize / 2 * l;
tnum1 = cntcb[i];
tnum2 = cntcb[i - icbsize / 2];
for (j = 0; j < l; j++) {
tmp = (tnum2 * q[j] + tnum1 * p[j]) / (tnum1 + tnum2);
rand = nrandom(&next);
q[j] = tmp + delta * rand;
p[j] = tmp - delta * rand;
}
}
}
}
}
if (icbsize == ecbsize)
break;
}
return;
}
void hessian(RawArray<const T,2> x, RawArray<T,4> hess) const {
// Temporary arrays and views
GEODE_ASSERT(x.sizes()==vec(n+3,d) && hess.sizes()==vec(n+3,4,d,d));
const auto sx = smallx.flat.raw(),
sv = smallv.flat.raw();
// Collect quadrature points
const int e = 1+8*d+8*d*(d-1);
Array<T,3> tq( n,quads,e,uninit);
Array<T,4> xq(vec(n,quads,e,d),uninit);
Array<T,4> vq(vec(n,quads,e,d),uninit);
for (int i=0;i<n;i++) {
T_INFO(i)
for (int q=0;q<quads;q++) {
const T s = samples[q],
t = t1+dt*s;
for (int j=0;j<e;j++)
tq(i,q,j) = t;
SPLINE_INFO(s)
for (int a=0;a<d;a++) {
X_INFO(i,a)
const T x = a0*x0+a1*x1+a2*x2+a3*x3,
v = b0*x0+b1*x1+b2*x2+b3*x3;
for (int j=0;j<e;j++) {
xq(i,q,j,a) = x;
vq(i,q,j,a) = v;
}
int j = 1;
for (int b=0;b<d;b++) {
const T xb = sx[b],
vb = sv[b];
xq(i,q,j++,a) -= xb;
xq(i,q,j++,a) += xb;
vq(i,q,j++,a) -= vb;
vq(i,q,j++,a) += vb;
xq(i,q,j ,a) -= xb;
vq(i,q,j++,a) -= vb;
xq(i,q,j ,a) -= xb;
vq(i,q,j++,a) += vb;
xq(i,q,j ,a) += xb;
vq(i,q,j++,a) -= vb;
xq(i,q,j ,a) += xb;
vq(i,q,j++,a) += vb;
for (int c=b+1;c<d;c++) {
const T xc = sx[c],
vc = sv[c];
xq(i,q,j++,a) -= xb+xc;
xq(i,q,j++,a) -= xb-xc;
xq(i,q,j++,a) += xb-xc;
xq(i,q,j++,a) += xb+xc;
vq(i,q,j++,a) -= vb+vc;
vq(i,q,j++,a) -= vb-vc;
vq(i,q,j++,a) += vb-vc;
vq(i,q,j++,a) += vb+vc;
vq(i,q,j++,a) -= sv[b];
xq(i,q,j ,a) -= sx[b];
vq(i,q,j++,a) += sv[b];
xq(i,q,j ,a) += sx[b];
vq(i,q,j++,a) -= sv[b];
xq(i,q,j ,a) += sx[b];
vq(i,q,j++,a) += sv[b];
}
}
}
}
}
// Compute energies
const auto Uq_ = U(tq.reshape_own(n*quads*d4),NdArray<const T>(q2shape,xq.flat),NdArray<const T>(q2shape,vq.flat));
GEODE_ASSERT(Uq_.size()==n*quads*d4);
const auto Uq = Uq_.reshape(n,quads,d4);
// Accumulate
grad.fill(0);
const auto inv_2s = GEODE_RAW_ALLOCA(d,Vector<T,2>);
for (int a=0;a<d;a++)
inv_2s[a] = vec(.5/sx[a],.5/sv[a]);
for (int i=0;i<n;i++) {
T_INFO(i)
for (int q=0;q<quads;q++) {
const T s = samples[q],
w = dt*weights[q];
SPLINE_INFO(s)
for (int b=0;b<d;b++) {
const T wx = w*inv_2s[b].x*(Uq(i,q,4*b+1)-Uq(i,q,4*b )),
wv = w*inv_2s[b].y*(Uq(i,q,4*b+3)-Uq(i,q,4*b+2));
grad(i ,b) += a0*wx+b0*wv;
grad(i+1,b) += a1*wx+b1*wv;
grad(i+2,b) += a2*wx+b2*wv;
grad(i+3,b) += a3*wx+b3*wv;
}
}
}
}
