void IIR_LPF::MakeFilter(uint fc, uint samplingrate, uint _order)
{
order = Limit(_order, maxorder, 1);
double wa = tan(M_PI * fc / samplingrate);
double wa2 = wa*wa;
if (fc > samplingrate / 2)
fc = samplingrate / 2 - 1000;
uint j;
int n = 1;
for (j=0; j<order; j++)
{
double zt = cos(n * M_PI / 4 / order);
double ia0j = 1. / (1. + 2. * wa * zt + wa2);
fn[j][0] = FX(-2. * (wa2 - 1.) * ia0j);
fn[j][1] = FX(-(1. - 2. * wa * zt + wa2) * ia0j);
fn[j][2] = FX(wa2 * ia0j);
fn[j][3] = FX(2. * wa2 * ia0j);
n += 2;
}
for (int ch=0; ch<nchs; ch++)
{
for (j=0; j<order; j++)
{
b[ch][j][0] = b[ch][j][1] = 0;
}
}
}
static void twin_pixmap_read_xform_32 (twin_xform_t *xform, twin_coord_t line)
{
twin_fixed_t dx, dy, sx, sy;
unsigned int wx, wy;
twin_a8_t pts[4][4];
twin_a8_t *dst = xform->span.a8;
twin_pixmap_t *pix = xform->pixmap;
twin_matrix_t *tfm = &xform->pixmap->transform;
/* for each row in the dest line */
dy = twin_int_to_fixed(line);
for (dx = 0; dx < twin_int_to_fixed(xform->width); dx += TWIN_FIXED_ONE) {
sx = _twin_matrix_fx(tfm, dx, dy) + FX(xform->src_x);
sy = _twin_matrix_fy(tfm, dx, dy) + FX(xform->src_y);
_get_pix_32(pts[0], pix, sx, sy);
_get_pix_32(pts[1], pix, sx + TWIN_FIXED_ONE, sy);
_get_pix_32(pts[2], pix, sx, sy + TWIN_FIXED_ONE);
_get_pix_32(pts[3], pix, sx + TWIN_FIXED_ONE, sy + TWIN_FIXED_ONE);
wx = sx & 0xffff;
wy = sy & 0xffff;
*(dst++) = _pix_saucemix(pts[0][0], pts[1][0],
pts[2][0], pts[3][0], wx, wy);
*(dst++) = _pix_saucemix(pts[0][1], pts[1][1],
pts[2][1], pts[3][1], wx, wy);
*(dst++) = _pix_saucemix(pts[0][2], pts[1][2],
pts[2][2], pts[3][2], wx, wy);
*(dst++) = _pix_saucemix(pts[0][3], pts[1][3],
pts[2][3], pts[3][3], wx, wy);
}
}
void pascal near draw_track(unsigned char sel, unsigned char color)
{
unsigned char other_page = 1 - music_page;
graph_accesspage(other_page);
graph_putsa_fx(16, (sel + 6) * 16, FX(color, 2, 0), MUSIC_TITLES[sel]);
graph_accesspage(music_page);
graph_putsa_fx(16, (sel + 6) * 16, FX(color, 2, 0), MUSIC_TITLES[sel]);
}
void pascal near draw_cmt(int track)
{
int line;
int p;
music_cmt_load(track);
screen_back_B_put();
cmt_back_put();
graph_putsa_fx(160, 64, FX(15, 1, 0), music_cmt[0]);
for(line = 1; line < MUSIC_CMT_LINE_COUNT; line++) {
graph_putsa_fx(304, (line + 4) * 16, FX(13, 1, 0), music_cmt[line]);
}
PLANE_DWORD_BLIT(screen_back_B, VRAM_PLANE_B);
}
Vector3<Real> ImplicitSurface<Real>::GetGradient (const Vector3<Real>& pos)
const
{
Real fx = FX(pos);
Real fy = FY(pos);
Real fz = FZ(pos);
return Vector3<Real>(fx, fy, fz);
}
Vector3<Real> ImplicitSurface<Real>::GetGradient (const Vector3<Real>& rkP)
const
{
Real fFX = FX(rkP);
Real fFY = FY(rkP);
Real fFZ = FZ(rkP);
return Vector3<Real>(fFX,fFY,fFZ);
}
double probability_x_less_than_y(const std::valarray<double>& x, const std::valarray<double>& y)
{
vector<double> x2(x.size());
for(int i=0;i<x2.size();i++)
x2[i] = x[i];
sort(x2.begin(),x2.end());
vector<double> y2(y.size());
for(int i=0;i<y2.size();i++)
y2[i] = y[i];
sort(y2.begin(),y2.end());
valarray<double> FX(y.size());
int dx=0;
for(int dy=0;dy<FX.size();dy++) {
while((dx<x2.size()) and (x2[dx]<y2[dy]))
dx++;
FX[dy] = double(dx)/x2.size();
}
return FX.sum()/FX.size();
}
FX ParallelizerInternal::getJacobian(int iind, int oind, bool compact, bool symmetric){
// Number of tasks
int ntask = inind_.size()-1;
// Find out which task corresponds to the iind
int task;
for(task=0; task<ntask && iind>=inind_[task+1]; ++task);
// Check if the output index is also in this task
if(oind>=outind_[task] && oind<outind_[task+1]){
// Get the Jacobian index
int iind_f = iind-inind_[task];
int oind_f = oind-outind_[task];
// Get the local jacobian
return funcs_.at(task).jacobian(iind_f,oind_f, compact, symmetric);
} else {
// All-zero jacobian
return FX();
}
}
NLPSolverInternal::NLPSolverInternal(const FX& F, const FX& G, const FX& H, const FX& J) : F_(F), G_(G), H_(H), J_(J){
// set default options
setOption("name", "unnamed NLP solver"); // name of the function
addOption("expand_f", OT_BOOLEAN, false, "Expand the objective function in terms of scalar operations, i.e. MX->SX");
addOption("expand_g", OT_BOOLEAN, false, "Expand the constraint function in terms of scalar operations, i.e. MX->SX");
addOption("generate_hessian", OT_BOOLEAN, false, "Generate an exact Hessian of the Lagrangian if not supplied");
addOption("generate_jacobian", OT_BOOLEAN, true, "Generate an exact Jacobian of the constraints if not supplied");
addOption("iteration_callback", OT_FX, FX(), "A function that will be called at each iteration. Input scheme is the same as NLPSolver's output scheme. Output is scalar.");
addOption("iteration_callback_step", OT_INTEGER, 1, "Only call the callback function every few iterations.");
addOption("iteration_callback_ignore_errors", OT_BOOLEAN, false, "If set to true, errors thrown by iteration_callback will be ignored.");
addOption("ignore_check_vec", OT_BOOLEAN, false, "If set to true, the input shape of F will not be checked.");
addOption("warn_initial_bounds", OT_BOOLEAN, false, "Warn if the initial guess does not satisfy LBX and UBX");
addOption("parametric", OT_BOOLEAN, false, "Expect F, G, H, J to have an additional input argument appended at the end, denoting fixed parameters.");
addOption("gauss_newton", OT_BOOLEAN, false, "Use Gauss Newton Hessian approximation");
n_ = 0;
m_ = 0;
inputScheme = SCHEME_NLPInput;
outputScheme = SCHEME_NLPOutput;
}
bool ImplicitSurface<Real>::ComputePrincipalCurvatureInfo (
const Vector3<Real>& pos, Real& curv0, Real& curv1, Vector3<Real>& dir0,
Vector3<Real>& dir1)
{
// Principal curvatures and directions for implicitly defined surfaces
// F(x,y,z) = 0.
//
// DF = (Fx,Fy,Fz), L = Length(DF)
//
// D^2 F = +- -+
// | Fxx Fxy Fxz |
// | Fxy Fyy Fyz |
// | Fxz Fyz Fzz |
// +- -+
//
// adj(D^2 F) = +- -+
// | Fyy*Fzz-Fyz*Fyz Fyz*Fxz-Fxy*Fzz Fxy*Fyz-Fxz*Fyy |
// | Fyz*Fxz-Fxy*Fzz Fxx*Fzz-Fxz*Fxz Fxy*Fxz-Fxx*Fyz |
// | Fxy*Fyz-Fxz*Fyy Fxy*Fxz-Fxx*Fyz Fxx*Fyy-Fxy*Fxy |
// +- -+
//
// Gaussian curvature = [DF^t adj(D^2 F) DF]/L^4
//
// Mean curvature = 0.5*[trace(D^2 F)/L - (DF^t D^2 F DF)/L^3]
// first derivatives
Real fx = FX(pos);
Real fy = FY(pos);
Real fz = FZ(pos);
Real fLength = Math<Real>::Sqrt(fx*fx + fy*fy + fz*fz);
if (fLength <= Math<Real>::ZERO_TOLERANCE)
{
return false;
}
Real fxfx = fx*fx;
Real fxfy = fx*fy;
Real fxfz = fx*fz;
Real fyfy = fy*fy;
Real fyfz = fy*fz;
Real fzfz = fz*fz;
Real invLength = ((Real)1)/fLength;
Real invLength2 = invLength*invLength;
Real invLength3 = invLength*invLength2;
Real invLength4 = invLength2*invLength2;
// second derivatives
Real fxx = FXX(pos);
Real fxy = FXY(pos);
Real fxz = FXZ(pos);
Real fyy = FYY(pos);
Real fyz = FYZ(pos);
Real fzz = FZZ(pos);
// mean curvature
Real meanCurv = ((Real)0.5)*invLength3*(fxx*(fyfy + fzfz) +
fyy*(fxfx + fzfz) + fzz*(fxfx + fyfy) - ((Real)2)*(fxy*fxfy + fxz*fxfz +
fyz*fyfz));
// Gaussian curvature
Real gaussCurv = invLength4*(fxfx*(fyy*fzz - fyz*fyz)
+ fyfy*(fxx*fzz - fxz*fxz) + fzfz*(fxx*fyy - fxy*fxy)
+ ((Real)2)*(fxfy*(fxz*fyz - fxy*fzz) + fxfz*(fxy*fyz - fxz*fyy)
+ fyfz*(fxy*fxz - fxx*fyz)));
// solve for principal curvatures
Real discr = Math<Real>::Sqrt(Math<Real>::FAbs(meanCurv*meanCurv-gaussCurv));
curv0 = meanCurv - discr;
curv1 = meanCurv + discr;
Real m00 = ((-(Real)1 + fxfx*invLength2)*fxx)*invLength +
(fxfy*fxy)*invLength3 + (fxfz*fxz)*invLength3;
Real m01 = ((-(Real)1 + fxfx*invLength2)*fxy)*invLength +
(fxfy*fyy)*invLength3 + (fxfz*fyz)*invLength3;
Real m02 = ((-(Real)1 + fxfx*invLength2)*fxz)*invLength +
(fxfy*fyz)*invLength3 + (fxfz*fzz)*invLength3;
Real m10 = (fxfy*fxx)*invLength3 +
((-(Real)1 + fyfy*invLength2)*fxy)*invLength + (fyfz*fxz)*invLength3;
Real m11 = (fxfy*fxy)*invLength3 +
((-(Real)1 + fyfy*invLength2)*fyy)*invLength + (fyfz*fyz)*invLength3;
Real m12 = (fxfy*fxz)*invLength3 +
((-(Real)1 + fyfy*invLength2)*fyz)*invLength + (fyfz*fzz)*invLength3;
Real m20 = (fxfz*fxx)*invLength3 + (fyfz*fxy)*invLength3 +
((-(Real)1 + fzfz*invLength2)*fxz)*invLength;
Real m21 = (fxfz*fxy)*invLength3 + (fyfz*fyy)*invLength3 +
((-(Real)1 + fzfz*invLength2)*fyz)*invLength;
Real m22 = (fxfz*fxz)*invLength3 + (fyfz*fyz)*invLength3 +
((-(Real)1 + fzfz*invLength2)*fzz)*invLength;
// solve for principal directions
Real tmp1 = m00 + curv0;
Real tmp2 = m11 + curv0;
Real tmp3 = m22 + curv0;
Vector3<Real> U[3];
Real lengths[3];
U[0].X() = m01*m12-m02*tmp2;
U[0].Y() = m02*m10-m12*tmp1;
U[0].Z() = tmp1*tmp2-m01*m10;
lengths[0] = U[0].Length();
U[1].X() = m01*tmp3-m02*m21;
U[1].Y() = m02*m20-tmp1*tmp3;
U[1].Z() = tmp1*m21-m01*m20;
lengths[1] = U[1].Length();
U[2].X() = tmp2*tmp3-m12*m21;
U[2].Y() = m12*m20-m10*tmp3;
U[2].Z() = m10*m21-m20*tmp2;
lengths[2] = U[2].Length();
int maxIndex = 0;
Real maxValue = lengths[0];
if (lengths[1] > maxValue)
{
maxIndex = 1;
maxValue = lengths[1];
}
if (lengths[2] > maxValue)
{
maxIndex = 2;
}
invLength = ((Real)1)/lengths[maxIndex];
U[maxIndex] *= invLength;
dir1 = U[maxIndex];
dir0 = dir1.UnitCross(Vector3<Real>(fx, fy, fz));
return true;
}
static void
tidy_texture_frame_paint (ClutterActor *self)
{
TidyTextureFramePrivate *priv;
ClutterActor *parent_texture;
guint width, height;
gint pwidth, pheight, ex, ey;
ClutterFixed tx1, ty1, tx2, ty2, tw, th;
GLenum target_type;
ClutterColor col = { 0xff, 0xff, 0xff, 0xff };
priv = TIDY_TEXTURE_FRAME (self)->priv;
/* no need to paint stuff if we don't have a texture to reflect */
if (!clutter_clone_texture_get_parent_texture (CLUTTER_CLONE_TEXTURE(self)))
return;
/* parent texture may have been hidden, there for need to make sure its
* realised with resources available.
*/
parent_texture
= CLUTTER_ACTOR
(clutter_clone_texture_get_parent_texture(CLUTTER_CLONE_TEXTURE(self)));
if (!CLUTTER_ACTOR_IS_REALIZED (parent_texture))
clutter_actor_realize (parent_texture);
if (clutter_texture_is_tiled (CLUTTER_TEXTURE (parent_texture)))
{
g_warning("tiled textures not yet supported...");
return;
}
cogl_push_matrix ();
#define FX(x) CLUTTER_INT_TO_FIXED(x)
clutter_texture_get_base_size (CLUTTER_TEXTURE(parent_texture),
&pwidth, &pheight);
clutter_actor_get_size (self, &width, &height);
tx1 = FX (priv->left);
tx2 = FX (pwidth - priv->right);
ty1 = FX (priv->top);
ty2 = FX (pheight - priv->bottom);
tw = FX (pwidth);
th = FX (pheight);
if (clutter_feature_available (CLUTTER_FEATURE_TEXTURE_RECTANGLE))
{
target_type = CGL_TEXTURE_RECTANGLE_ARB;
cogl_enable (CGL_ENABLE_TEXTURE_RECT|CGL_ENABLE_BLEND);
}
else
{
target_type = CGL_TEXTURE_2D;
cogl_enable (CGL_ENABLE_TEXTURE_2D|CGL_ENABLE_BLEND);
tw = clutter_util_next_p2 (pwidth);
th = clutter_util_next_p2 (pheight);
tx1 = tx1/tw;
tx2 = tx2/tw;
ty1 = ty1/th;
ty2 = ty2/th;
tw = FX(pwidth)/tw;
th = FX(pheight)/th;
}
col.alpha = clutter_actor_get_opacity (self);
cogl_color (&col);
clutter_texture_bind_tile (CLUTTER_TEXTURE(parent_texture), 0);
ex = width - priv->right;
if (ex < 0)
ex = priv->right; /* FIXME */
ey = height - priv->bottom;
if (ey < 0)
ey = priv->bottom; /* FIXME */
/* top left corner */
cogl_texture_quad (0,
priv->left, /* FIXME: clip if smaller */
0,
priv->top,
0,
0,
tx1,
ty1);
/* top middle */
cogl_texture_quad (priv->left,
ex,
0,
priv->top,
tx1,
0,
tx2,
ty1);
/* top right */
cogl_texture_quad (ex,
width,
0,
priv->top,
tx2,
0,
tw,
ty1);
/* mid left */
cogl_texture_quad (0,
priv->left,
priv->top,
ey,
0,
ty1,
tx1,
ty2);
/* center */
cogl_texture_quad (priv->left,
ex,
priv->top,
ey,
tx1,
ty1,
tx2,
ty2);
/* mid right */
cogl_texture_quad (ex,
width,
priv->top,
ey,
tx2,
ty1,
tw,
ty2);
/* bottom left */
cogl_texture_quad (0,
priv->left,
ey,
height,
0,
ty2,
tx1,
th);
/* bottom center */
cogl_texture_quad (priv->left,
ex,
ey,
height,
tx1,
ty2,
tx2,
th);
/* bottom right */
cogl_texture_quad (ex,
width,
ey,
height,
tx2,
ty2,
tw,
th);
cogl_pop_matrix ();
#undef FX
}
#define ROT13_SpC 201373 #else #include <math.h> #define FX(x) ( (int)floor((x)*(1<<FIX) + .5 ) ) static const double c1 = cos(1.*M_PI/16); static const double c2 = cos(2.*M_PI/16); static const double c3 = cos(3.*M_PI/16); static const double c4 = cos(4.*M_PI/16); static const double c5 = cos(5.*M_PI/16); static const double c6 = cos(6.*M_PI/16); static const double c7 = cos(7.*M_PI/16); static const int ROT6_C = FX(c2-c6); // 0.541 static const int ROT6_SmC = FX(2*c6); // 0.765 static const int ROT6_SpC = FX(2*c2); // 1.847 static const int ROT17_C = FX(c1+c7); // 1.175 static const int ROT17_SmC = FX(2*c7); // 0.390 static const int ROT17_SpC = FX(2*c1); // 1.961 static const int ROT37_C = FX((c3-c7)/c4); // 0.899 static const int ROT37_SmC = FX(2*(c5+c7)); // 1.501 static const int ROT37_SpC = FX(2*(c1-c3)); // 0.298 static const int ROT13_C = FX((c1+c3)/c4); // 2.562 static const int ROT13_SmC = FX(2*(c3+c7)); // 2.053 static const int ROT13_SpC = FX(2*(c1+c5)); // 3.072
void AdvectionManager::advance(double dt, const TimeLevelIndex<2> &newTimeIdx,
const VelocityField &velocity) {
TimeLevelIndex<2> oldTimeIdx = newTimeIdx-1;
TimeLevelIndex<2> halfTimeIdx = newTimeIdx-0.5;
const double eps = 1.0e-80;
const double R = domain->getRadius();
for (int s = 0; s < Q.size(); ++s) {
ScalarField &q = *Q[s];
double dQ;
int J;
//#define ONLY_LAX_WENDROFF
//#define ONLY_UPWIND
#ifndef ONLY_UPWIND
// ---------------------------------------------------------------------
// Lax-Wendroff pass
for (int j = 1; j < mesh->getNumGrid(1, FULL)-1; ++j) {
for (int i = 0; i < mesh->getNumGrid(0, HALF); ++i) {
double f1 = dt/(R*dlon[i]);
double f2 = f1/cosLatFull[j];
double u = velocity(0)(halfTimeIdx, i, j);
double q1 = q(oldTimeIdx, i, j);
double q2 = q(oldTimeIdx, i+1, j);
FX(i, j) = 0.5*f1*(u*(q2+q1)-u*u*f2*(q2-q1));
}
}
FX.applyBndCond();
for (int j = 0; j < mesh->getNumGrid(1, HALF); ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
double f1 = dt/(R*dlat[j]);
double f2 = f1/cosLatHalf[j];
double v = velocity(1)(halfTimeIdx, i, j)*cosLatHalf[j];
double q1 = q(oldTimeIdx, i, j );
double q2 = q(oldTimeIdx, i, j+1);
FY(i, j) = 0.5*f1*(v*(q2+q1)-v*v*f2*(q2-q1));
}
}
FY.applyBndCond();
#endif
#if (!defined ONLY_LAX_WENDROFF && !defined ONLY_UPWIND)
// ---------------------------------------------------------------------
// calculate intermediate Qstar
for (int j = 1; j < mesh->getNumGrid(1, FULL)-1; ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
double fx1 = dt/(R*dlon[i]*cosLatFull[i]); // TODO: Should we support irregular lon grids?
double fx2 = dt/(R*dlon[i]*cosLatFull[i]);
double fy1 = dt/(R*dlat[j-1]*cosLatHalf[j-1]);
double fy2 = dt/(R*dlat[j ]*cosLatHalf[j ]);
double u1 = velocity(0)(halfTimeIdx, i-1, j);
double u2 = velocity(0)(halfTimeIdx, i, j);
double v1 = velocity(1)(halfTimeIdx, i, j-1);
double v2 = velocity(1)(halfTimeIdx, i, j );
double tmp1 = fabs(u1*fx1)*(1-fabs(u1*fx1));
double tmp2 = fabs(u2*fx2)*(1-fabs(u2*fx2));
double tmp3 = fabs(v1*fy1)*(1-fabs(v1*fy1));
double tmp4 = fabs(v2*fy2)*(1-fabs(v2*fy2));
double gamma = fmax(fmax(tmp1, tmp2), fmax(tmp3, tmp4));
B(i, j) = 2/(2-2*gamma);
}
}
for (int j = 1; j < mesh->getNumGrid(1, FULL)-1; ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
Qstar(i, j) = q(oldTimeIdx, i, j)-B(i, j)/cosLatFull[j]*
(FX(i, j)-FX(i-1, j)+FY(i, j)-FY(i, j-1));
}
}
// handle poles
// south pole
dQ = 0; J = 0;
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
dQ += B(i, J+1)*FY(i, J);
}
dQ *= 4.0/mesh->getNumGrid(0, FULL)/cosLatHalf[J];
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
Qstar(i, J) = q(oldTimeIdx, i, J)-dQ;
}
// north pole
dQ = 0; J = mesh->getNumGrid(1, FULL)-1;
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
dQ += B(i, J-1)*FY(i, J-1);
}
dQ *= 4.0/mesh->getNumGrid(0, FULL)/cosLatHalf[J-1];
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
Qstar(i, J) = q(oldTimeIdx, i, J)+dQ;
}
// ---------------------------------------------------------------------
// shape-preserving rule (A <= 0 is good)
for (int j = 0; j < mesh->getNumGrid(1, FULL); ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
double Qmin = 1.0e+15;
double Qmax = -1.0e+15;
if (j == 0) {
Qmin = fmin(fmin(q(oldTimeIdx, i, j), q(oldTimeIdx, i, j+1)), Qmin);
Qmax = fmax(fmax(q(oldTimeIdx, i, j), q(oldTimeIdx, i, j+1)), Qmax);
} else if (j == mesh->getNumGrid(1, FULL)-1) {
Qmin = fmin(fmin(q(oldTimeIdx, i, j), q(oldTimeIdx, i, j-1)), Qmin);
Qmax = fmax(fmax(q(oldTimeIdx, i, j), q(oldTimeIdx, i, j-1)), Qmax);
} else {
Qmin = fmin(fmin(fmin(q(oldTimeIdx, i-1, j), q(oldTimeIdx, i+1, j)),
fmin(q(oldTimeIdx, i, j-1), q(oldTimeIdx, i, j+1))),
fmin(q(oldTimeIdx, i, j), Qmin));
Qmax = fmax(fmax(fmax(q(oldTimeIdx, i-1, j), q(oldTimeIdx, i+1, j)),
fmax(q(oldTimeIdx, i, j-1), q(oldTimeIdx, i, j+1))),
fmax(q(oldTimeIdx, i, j), Qmax));
}
A(i, j) = (Qstar(i, j)-Qmax)*(Qstar(i, j)-Qmin);
}
}
A.applyBndCond();
#endif
#ifndef ONLY_LAX_WENDROFF
// ---------------------------------------------------------------------
// upwind pass
for (int j = 1; j < mesh->getNumGrid(1, FULL)-1; ++j) {
for (int i = 0; i < mesh->getNumGrid(0, HALF); ++i) {
#ifndef ONLY_UPWIND
double tmp1 = (fabs(A(i, j))+A(i, j))/(fabs(A(i, j))+eps);
double tmp2 = (fabs(A(i+1, j))+A(i+1, j))/(fabs(A(i+1, j))+eps);
double tmp3 = (fabs(A(i+1, j))+A(i+1, j))*(fabs(A(i, j))+A(i, j));
double tmp4 = fabs(A(i, j))*fabs(A(i+1, j))+eps;
double cxstar = 0.5*(tmp1+tmp2)-0.25*tmp3/tmp4;
#else
double cxstar = 1;
#endif
double f = dt/(R*dlon[i]);
double u = velocity(0)(halfTimeIdx, i, j);
double q1 = q(oldTimeIdx, i, j);
double q2 = q(oldTimeIdx, i+1, j);
double cx = cxstar+(1-cxstar)*fabs(u*f/cosLatFull[j]);
FX(i, j) = 0.5*f*(u*(q2+q1)-fabs(cx*u)*(q2-q1));
}
}
FX.applyBndCond();
for (int j = 0; j < mesh->getNumGrid(1, HALF); ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
#ifndef ONLY_UPWIND
double tmp1 = (fabs(A(i, j ))+A(i, j ))/(fabs(A(i, j ))+eps);
double tmp2 = (fabs(A(i, j+1))+A(i, j+1))/(fabs(A(i, j+1))+eps);
double tmp3 = (fabs(A(i, j+1))+A(i, j+1))*(fabs(A(i, j))+A(i, j));
double tmp4 = fabs(A(i, j))*fabs(A(i, j+1))+eps;
double cystar = 0.5*(tmp1+tmp2)-0.25*tmp3/tmp4;
#else
double cystar = 1;
#endif
double f = dt/(R*dlat[j]);
double v = velocity(1)(halfTimeIdx, i, j)*cosLatHalf[j];
double q1 = q(oldTimeIdx, i, j );
double q2 = q(oldTimeIdx, i, j+1);
double cy = cystar+(1-cystar)*fabs(v*f/cosLatHalf[j]);
FY(i, j) = 0.5*f*(v*(q2+q1)-fabs(cy*v)*(q2-q1));
}
}
FY.applyBndCond();
#endif
// ---------------------------------------------------------------------
// calculate final Q
for (int j = 1; j < mesh->getNumGrid(1, FULL)-1; ++j) {
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
q(newTimeIdx, i, j) = q(oldTimeIdx, i, j)-
(FX(i, j)-FX(i-1, j)+FY(i, j)-FY(i, j-1))/cosLatFull[j];
}
}
// handle poles
// south pole
dQ = 0; J = 0;
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
dQ += FY(i, J);
}
dQ *= 4.0/mesh->getNumGrid(0, FULL)/cosLatHalf[J];
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
q(newTimeIdx, i, J) = q(oldTimeIdx, i, J)-dQ;
}
// north pole
dQ = 0; J = mesh->getNumGrid(1, FULL)-1;
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
dQ += FY(i, J-1);
}
dQ *= 4.0/mesh->getNumGrid(0, FULL)/cosLatHalf[J-1];
for (int i = 0; i < mesh->getNumGrid(0, FULL); ++i) {
q(newTimeIdx, i, J) = q(oldTimeIdx, i, J)+dQ;
}
q.applyBndCond(newTimeIdx);
}
diagnose(newTimeIdx);
}
bool ImplicitSurface<Real>::ComputePrincipalCurvatureInfo (
const Vector3<Real>& rkP, Real& rfCurv0, Real& rfCurv1,
Vector3<Real>& rkDir0, Vector3<Real>& rkDir1)
{
// Principal curvatures and directions for implicitly defined surfaces
// F(x,y,z) = 0.
//
// DF = (Fx,Fy,Fz), L = Length(DF)
//
// D^2 F = +- -+
// | Fxx Fxy Fxz |
// | Fxy Fyy Fyz |
// | Fxz Fyz Fzz |
// +- -+
//
// adj(D^2 F) = +- -+
// | Fyy*Fzz-Fyz*Fyz Fyz*Fxz-Fxy*Fzz Fxy*Fyz-Fxz*Fyy |
// | Fyz*Fxz-Fxy*Fzz Fxx*Fzz-Fxz*Fxz Fxy*Fxz-Fxx*Fyz |
// | Fxy*Fyz-Fxz*Fyy Fxy*Fxz-Fxx*Fyz Fxx*Fyy-Fxy*Fxy |
// +- -+
//
// Gaussian curvature = [DF^t adj(D^2 F) DF]/L^4
//
// Mean curvature = 0.5*[trace(D^2 F)/L - (DF^t D^2 F DF)/L^3]
// first derivatives
Real fFX = FX(rkP);
Real fFY = FY(rkP);
Real fFZ = FZ(rkP);
Real fL = Math<Real>::Sqrt(fFX*fFX + fFY*fFY + fFZ*fFZ);
if (fL <= Math<Real>::ZERO_TOLERANCE)
{
return false;
}
Real fFXFX = fFX*fFX;
Real fFXFY = fFX*fFY;
Real fFXFZ = fFX*fFZ;
Real fFYFY = fFY*fFY;
Real fFYFZ = fFY*fFZ;
Real fFZFZ = fFZ*fFZ;
Real fInvL = ((Real)1.0)/fL;
Real fInvL2 = fInvL*fInvL;
Real fInvL3 = fInvL*fInvL2;
Real fInvL4 = fInvL2*fInvL2;
// second derivatives
Real fFXX = FXX(rkP);
Real fFXY = FXY(rkP);
Real fFXZ = FXZ(rkP);
Real fFYY = FYY(rkP);
Real fFYZ = FYZ(rkP);
Real fFZZ = FZZ(rkP);
// mean curvature
Real fMCurv = ((Real)0.5)*fInvL3*(fFXX*(fFYFY+fFZFZ) + fFYY*(fFXFX+fFZFZ)
+ fFZZ*(fFXFX+fFYFY)
- ((Real)2.0)*(fFXY*fFXFY+fFXZ*fFXFZ+fFYZ*fFYFZ));
// Gaussian curvature
Real fGCurv = fInvL4*(fFXFX*(fFYY*fFZZ-fFYZ*fFYZ)
+ fFYFY*(fFXX*fFZZ-fFXZ*fFXZ) + fFZFZ*(fFXX*fFYY-fFXY*fFXY)
+ ((Real)2.0)*(fFXFY*(fFXZ*fFYZ-fFXY*fFZZ)
+ fFXFZ*(fFXY*fFYZ-fFXZ*fFYY)
+ fFYFZ*(fFXY*fFXZ-fFXX*fFYZ)));
// solve for principal curvatures
Real fDiscr = Math<Real>::Sqrt(Math<Real>::FAbs(fMCurv*fMCurv-fGCurv));
rfCurv0 = fMCurv - fDiscr;
rfCurv1 = fMCurv + fDiscr;
Real fM00 = ((-(Real)1.0 + fFXFX*fInvL2)*fFXX)*fInvL + (fFXFY*fFXY)*fInvL3
+ (fFXFZ*fFXZ)*fInvL3;
Real fM01 = ((-(Real)1.0 + fFXFX*fInvL2)*fFXY)*fInvL + (fFXFY*fFYY)*fInvL3
+ (fFXFZ*fFYZ)*fInvL3;
Real fM02 = ((-(Real)1.0 + fFXFX*fInvL2)*fFXZ)*fInvL + (fFXFY*fFYZ)*fInvL3
+ (fFXFZ*fFZZ)*fInvL3;
Real fM10 = (fFXFY*fFXX)*fInvL3 + ((-(Real)1.0 + fFYFY*fInvL2)*fFXY)*fInvL
+ (fFYFZ*fFXZ)*fInvL3;
Real fM11 = (fFXFY*fFXY)*fInvL3 + ((-(Real)1.0 + fFYFY*fInvL2)*fFYY)*fInvL
+ (fFYFZ*fFYZ)*fInvL3;
Real fM12 = (fFXFY*fFXZ)*fInvL3 + ((-(Real)1.0 + fFYFY*fInvL2)*fFYZ)*fInvL
+ (fFYFZ*fFZZ)*fInvL3;
Real fM20 = (fFXFZ*fFXX)*fInvL3 + (fFYFZ*fFXY)*fInvL3 + ((-(Real)1.0
+ fFZFZ*fInvL2)*fFXZ)*fInvL;
Real fM21 = (fFXFZ*fFXY)*fInvL3 + (fFYFZ*fFYY)*fInvL3 + ((-(Real)1.0
+ fFZFZ*fInvL2)*fFYZ)*fInvL;
Real fM22 = (fFXFZ*fFXZ)*fInvL3 + (fFYFZ*fFYZ)*fInvL3 + ((-(Real)1.0
+ fFZFZ*fInvL2)*fFZZ)*fInvL;
// solve for principal directions
Real fTmp1 = fM00 + rfCurv0;
Real fTmp2 = fM11 + rfCurv0;
Real fTmp3 = fM22 + rfCurv0;
Vector3<Real> akU[3];
Real afLength[3];
akU[0].X() = fM01*fM12-fM02*fTmp2;
akU[0].Y() = fM02*fM10-fM12*fTmp1;
akU[0].Z() = fTmp1*fTmp2-fM01*fM10;
afLength[0] = akU[0].Length();
akU[1].X() = fM01*fTmp3-fM02*fM21;
akU[1].Y() = fM02*fM20-fTmp1*fTmp3;
akU[1].Z() = fTmp1*fM21-fM01*fM20;
afLength[1] = akU[1].Length();
akU[2].X() = fTmp2*fTmp3-fM12*fM21;
akU[2].Y() = fM12*fM20-fM10*fTmp3;
akU[2].Z() = fM10*fM21-fM20*fTmp2;
afLength[2] = akU[2].Length();
int iMaxIndex = 0;
Real fMax = afLength[0];
if (afLength[1] > fMax)
{
iMaxIndex = 1;
fMax = afLength[1];
}
if (afLength[2] > fMax)
{
iMaxIndex = 2;
}
Real fInvLength = ((Real)1.0)/afLength[iMaxIndex];
akU[iMaxIndex] *= fInvLength;
rkDir1 = akU[iMaxIndex];
rkDir0 = rkDir1.UnitCross(Vector3<Real>(fFX,fFY,fFZ));
return true;
}
void DirectSingleShootingInternal::init(){
// Initialize the base classes
OCPSolverInternal::init();
// Create an integrator instance
integratorCreator integrator_creator = getOption("integrator");
integrator_ = integrator_creator(ffcn_,FX());
if(hasSetOption("integrator_options")){
integrator_.setOption(getOption("integrator_options"));
}
// Set t0 and tf
integrator_.setOption("t0",0);
integrator_.setOption("tf",tf_/nk_);
integrator_.init();
// Path constraints present?
bool path_constraints = nh_>0;
// Count the total number of NLP variables
int NV = np_ + // global parameters
nx_ + // initial state
nu_*nk_; // local control
// Declare variable vector for the NLP
// The structure is as follows:
// np x 1 (parameters)
// ------
// nx x 1 (states at time i=0)
// ------
// nu x 1 (controls in interval i=0)
// .....
// nx x 1 (controls in interval i=nk-1)
MX V = msym("V",NV);
int offset = 0;
// Global parameters
MX P = V[Slice(0,np_)];
offset += np_;
// Initial state
MX X0 = V[Slice(offset,offset+nx_)];
offset += nx_;
// Control for each shooting interval
vector<MX> U(nk_);
for(int k=0; k<nk_; ++k){ // interior nodes
U[k] = V[range(offset,offset+nu_)];
offset += nu_;
}
// Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
casadi_assert(offset==NV);
// Current state
MX X = X0;
// Objective
MX nlp_j = 0;
// Constraints
vector<MX> nlp_g;
nlp_g.reserve(nk_*(path_constraints ? 2 : 1));
// For all shooting nodes
for(int k=0; k<nk_; ++k){
// Integrate
vector<MX> int_out = integrator_.call(integratorIn("x0",X,"p",vertcat(P,U[k])));
// Store expression for state trajectory
X = int_out[INTEGRATOR_XF];
// Add constraints on the state
nlp_g.push_back(X);
// Add path constraints
if(path_constraints){
vector<MX> cfcn_out = cfcn_.call(daeIn("x",X,"p",U[k])); // TODO: Change signature of cfcn_: remove algebraic variable, add control
nlp_g.push_back(cfcn_out.at(0));
}
}
// Terminal constraints
G_ = MXFunction(V,vertcat(nlp_g));
G_.setOption("name","nlp_g");
G_.init();
// Objective function
MX jk = mfcn_.call(mayerIn("x",X,"p",P)).at(0);
nlp_j += jk;
F_ = MXFunction(V,nlp_j);
F_.setOption("name","nlp_j");
// Get the NLP creator function
NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
// Allocate an NLP solver
nlp_solver_ = nlp_solver_creator(F_,G_,FX(),FX());
// Pass options
if(hasSetOption("nlp_solver_options")){
const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
nlp_solver_.setOption(nlp_solver_options);
}
// Initialize the solver
nlp_solver_.init();
}
static void
bloops_synth(int length, float* buffer)
{
int bi, t, i, si;
while (length--)
{
int samplecount = 0;
float allsample = 0.0f;
for (bi = 0; bi < BLOOPS_MAX_CHANNELS; bi++)
{
int moreframes = 0;
bloops *B = MIXER->B[bi];
if (B == NULL)
continue;
for (t = 0; t < BLOOPS_MAX_TRACKS; t++)
{
bloopsavoice *A = &B->voices[t];
bloopsatrack *track = A->track;
if (track == NULL)
continue;
if (track->notes)
{
if (A->frames == A->nextnote[0])
{
if (A->nextnote[1] < track->nlen)
{
bloopsanote *note = &track->notes[A->nextnote[1]];
float freq = A->params.freq;
if (note->tone != 'n')
freq = bloops_note_freq(note->tone, (int)note->octave);
if (freq == 0.0f) {
A->period = 0.0f;
A->state = BLOOPS_STOP;
} else {
bloopsanote *note = &track->notes[A->nextnote[1]];
bloopsafx *fx = note->FX;
while (fx) {
switch (fx->cmd) {
case BLOOPS_FX_VOLUME: FX(fx, A->params.volume); break;
case BLOOPS_FX_PUNCH: FX(fx, A->params.punch); break;
case BLOOPS_FX_ATTACK: FX(fx, A->params.attack); break;
case BLOOPS_FX_SUSTAIN: FX(fx, A->params.sustain); break;
case BLOOPS_FX_DECAY: FX(fx, A->params.decay); break;
case BLOOPS_FX_SQUARE: FX(fx, A->params.square); break;
case BLOOPS_FX_SWEEP: FX(fx, A->params.sweep); break;
case BLOOPS_FX_VIBE: FX(fx, A->params.vibe); break;
case BLOOPS_FX_VSPEED: FX(fx, A->params.vspeed); break;
case BLOOPS_FX_VDELAY: FX(fx, A->params.vdelay); break;
case BLOOPS_FX_LPF: FX(fx, A->params.lpf); break;
case BLOOPS_FX_LSWEEP: FX(fx, A->params.lsweep); break;
case BLOOPS_FX_RESONANCE: FX(fx, A->params.resonance); break;
case BLOOPS_FX_HPF: FX(fx, A->params.hpf); break;
case BLOOPS_FX_HSWEEP: FX(fx, A->params.hsweep); break;
case BLOOPS_FX_ARP: FX(fx, A->params.arp); break;
case BLOOPS_FX_ASPEED: FX(fx, A->params.aspeed); break;
case BLOOPS_FX_PHASE: FX(fx, A->params.phase); break;
case BLOOPS_FX_PSWEEP: FX(fx, A->params.psweep); break;
case BLOOPS_FX_REPEAT: FX(fx, A->params.repeat); break;
}
fx = fx->next;
}
bloops_reset_voice(A);
bloops_start_voice(A);
A->period = 100.0 / (freq * freq + 0.001);
}
float length = 4.0f / note->duration;
A->nextnote[0] += (int)(tempo2frames(B->tempo) * (length + length * (1 - 1.0f / (1 << note->dotted))));
}
A->nextnote[1]++;
}
if (A->nextnote[1] <= track->nlen)
moreframes++;
}
else
{
moreframes++;
}
A->frames++;
if (A->state == BLOOPS_STOP)
continue;
samplecount++;
A->repeat++;
if (A->limit != 0 && A->repeat >= A->limit)
{
A->repeat = 0;
bloops_reset_voice(A);
}
A->atime++;
if (A->alimit != 0 && A->atime >= A->alimit)
{
A->alimit = 0;
A->period *= A->arp;
}
A->slide += A->dslide;
A->period *= A->slide;
if (A->period > A->maxperiod)
{
A->period = A->maxperiod;
if (A->params.limit > 0.0f)
A->state = BLOOPS_STOP;
}
float rfperiod = A->period;
if (A->vdelay > 0.0f)
{
A->vibe += A->vspeed;
rfperiod = A->period * (1.0 + sin(A->vibe) * A->vdelay);
}
int period = (int)rfperiod;
if (period < 8) period = 8;
A->square += A->sweep;
if(A->square < 0.0f) A->square = 0.0f;
if(A->square > 0.5f) A->square = 0.5f;
A->time++;
while (A->time >= A->length[A->stage])
{
A->time = 0;
A->stage++;
if (A->stage == 3)
A->state = BLOOPS_STOP;
}
switch (A->stage) {
case 0:
A->volume = (float)A->time / A->length[0];
break;
case 1:
A->volume = 1.0f + (1.0f - (float)A->time / A->length[1]) * 2.0f * A->params.punch;
break;
case 2:
A->volume = 1.0f - (float)A->time / A->length[2];
break;
}
A->fphase += A->dphase;
A->iphase = abs((int)A->fphase);
if (A->iphase > 1023) A->iphase = 1023;
if (A->filter[7] != 0.0f)
{
A->filter[6] *= A->filter[7];
if (A->filter[6] < 0.00001f) A->filter[6] = 0.00001f;
if (A->filter[6] > 0.1f) A->filter[6] = 0.1f;
}
float ssample = 0.0f;
for (si = 0; si < 8; si++)
{
float sample = 0.0f;
A->phase++;
if (A->phase >= period)
{
A->phase %= period;
if (A->params.type == BLOOPS_NOISE)
for (i = 0; i < 32; i++)
A->noise[i] = frnd(2.0f) - 1.0f;
}
float fp = (float)A->phase / period;
switch (A->params.type)
{
case BLOOPS_SQUARE:
if (fp < A->square)
sample = 0.5f;
else
sample = -0.5f;
break;
case BLOOPS_SAWTOOTH:
sample = 1.0f - fp * 2;
break;
case BLOOPS_SINE:
sample = (float)sin(fp * 2 * PI);
break;
case BLOOPS_NOISE:
sample = A->noise[A->phase * 32 / period];
break;
}
float pp = A->filter[0];
A->filter[2] *= A->filter[3];
if (A->filter[2] < 0.0f) A->filter[2] = 0.0f;
if (A->filter[2] > 0.1f) A->filter[2] = 0.1f;
if (A->params.lpf != 1.0f)
{
A->filter[1] += (sample - A->filter[0]) * A->filter[2];
A->filter[1] -= A->filter[1] * A->filter[4];
}
else
{
A->filter[0] = sample;
A->filter[1] = 0.0f;
}
A->filter[0] += A->filter[1];
A->filter[5] += A->filter[0] - pp;
A->filter[5] -= A->filter[5] * A->filter[6];
sample = A->filter[5];
A->phaser[A->phasex & 1023] = sample;
sample += A->phaser[(A->phasex - A->iphase + 1024) & 1023];
A->phasex = (A->phasex + 1) & 1023;
ssample += sample * A->volume;
}
ssample = ssample / 8 * B->volume;
ssample *= 2.0f * A->params.volume;
if (ssample > 1.0f) ssample = 1.0f;
if (ssample < -1.0f) ssample = -1.0f;
allsample += ssample;
}
if (moreframes == 0)
B->state = BLOOPS_STOP;
}
*buffer++ = allsample;
}
}
void DirectMultipleShootingInternal::init(){
// Initialize the base classes
OCPSolverInternal::init();
// Create an integrator instance
integratorCreator integrator_creator = getOption("integrator");
integrator_ = integrator_creator(ffcn_,FX());
if(hasSetOption("integrator_options")){
integrator_.setOption(getOption("integrator_options"));
}
// Set t0 and tf
integrator_.setOption("t0",0);
integrator_.setOption("tf",tf_/nk_);
integrator_.init();
// Path constraints present?
bool path_constraints = nh_>0;
// Count the total number of NLP variables
int NV = np_ + // global parameters
nx_*(nk_+1) + // local state
nu_*nk_; // local control
// Declare variable vector for the NLP
// The structure is as follows:
// np x 1 (parameters)
// ------
// nx x 1 (states at time i=0)
// nu x 1 (controls in interval i=0)
// ------
// nx x 1 (states at time i=1)
// nu x 1 (controls in interval i=1)
// ------
// .....
// ------
// nx x 1 (states at time i=nk)
MX V("V",NV);
// Global parameters
MX P = V(Slice(0,np_));
// offset in the variable vector
int v_offset=np_;
// Disretized variables for each shooting node
vector<MX> X(nk_+1), U(nk_);
for(int k=0; k<=nk_; ++k){ // interior nodes
// Local state
X[k] = V[Slice(v_offset,v_offset+nx_)];
v_offset += nx_;
// Variables below do not appear at the end point
if(k==nk_) break;
// Local control
U[k] = V[Slice(v_offset,v_offset+nu_)];
v_offset += nu_;
}
// Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
casadi_assert(v_offset==NV);
// Input to the parallel integrator evaluation
vector<vector<MX> > int_in(nk_);
for(int k=0; k<nk_; ++k){
int_in[k].resize(INTEGRATOR_NUM_IN);
int_in[k][INTEGRATOR_P] = vertcat(P,U[k]);
int_in[k][INTEGRATOR_X0] = X[k];
}
// Input to the parallel function evaluation
vector<vector<MX> > fcn_in(nk_);
for(int k=0; k<nk_; ++k){
fcn_in[k].resize(DAE_NUM_IN);
fcn_in[k][DAE_T] = (k*tf_)/nk_;
fcn_in[k][DAE_P] = vertcat(P,U.at(k));
fcn_in[k][DAE_X] = X[k];
}
// Options for the parallelizer
Dictionary paropt;
// Transmit parallelization mode
if(hasSetOption("parallelization"))
paropt["parallelization"] = getOption("parallelization");
// Evaluate function in parallel
vector<vector<MX> > pI_out = integrator_.call(int_in,paropt);
// Evaluate path constraints in parallel
vector<vector<MX> > pC_out;
if(path_constraints)
pC_out = cfcn_.call(fcn_in,paropt);
//Constraint function
vector<MX> gg(2*nk_);
// Collect the outputs
for(int k=0; k<nk_; ++k){
//append continuity constraints
gg[2*k] = pI_out[k][INTEGRATOR_XF] - X[k+1];
// append the path constraints
if(path_constraints)
gg[2*k+1] = pC_out[k][0];
}
// Terminal constraints
MX g = vertcat(gg);
// Objective function
MX f;
if (mfcn_.getNumInputs()==1) {
f = mfcn_.call(X.back()).front();
} else {
vector<MX> mfcn_argin(MAYER_NUM_IN);
mfcn_argin[MAYER_X] = X.back();
mfcn_argin[MAYER_P] = P;
f = mfcn_.call(mfcn_argin).front();
}
// NLP
nlp_ = MXFunction(nlpIn("x",V),nlpOut("f",f,"g",g));
nlp_.setOption("ad_mode","forward");
nlp_.init();
// Get the NLP creator function
NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
// Allocate an NLP solver
nlp_solver_ = nlp_solver_creator(nlp_);
// Pass user options
if(hasSetOption("nlp_solver_options")){
const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
nlp_solver_.setOption(nlp_solver_options);
}
// Initialize the solver
nlp_solver_.init();
}
FX GslInternal::getJacobian() {return FX();}
void DirectCollocationInternal::init(){
// Initialize the base classes
OCPSolverInternal::init();
// Free parameters currently not supported
casadi_assert_message(np_==0, "Not implemented");
// Legendre collocation points
double legendre_points[][6] = {
{0},
{0,0.500000},
{0,0.211325,0.788675},
{0,0.112702,0.500000,0.887298},
{0,0.069432,0.330009,0.669991,0.930568},
{0,0.046910,0.230765,0.500000,0.769235,0.953090}};
// Radau collocation points
double radau_points[][6] = {
{0},
{0,1.000000},
{0,0.333333,1.000000},
{0,0.155051,0.644949,1.000000},
{0,0.088588,0.409467,0.787659,1.000000},
{0,0.057104,0.276843,0.583590,0.860240,1.000000}};
// Read options
bool use_radau;
if(getOption("collocation_scheme")=="radau"){
use_radau = true;
} else if(getOption("collocation_scheme")=="legendre"){
use_radau = false;
}
// Interpolation order
deg_ = getOption("interpolation_order");
// All collocation time points
double* tau_root = use_radau ? radau_points[deg_] : legendre_points[deg_];
// Size of the finite elements
double h = tf_/nk_;
// Coefficients of the collocation equation
vector<vector<MX> > C(deg_+1,vector<MX>(deg_+1));
// Coefficients of the collocation equation as DMatrix
DMatrix C_num = DMatrix(deg_+1,deg_+1,0);
// Coefficients of the continuity equation
vector<MX> D(deg_+1);
// Coefficients of the collocation equation as DMatrix
DMatrix D_num = DMatrix(deg_+1,1,0);
// Collocation point
SXMatrix tau = ssym("tau");
// For all collocation points
for(int j=0; j<deg_+1; ++j){
// Construct Lagrange polynomials to get the polynomial basis at the collocation point
SXMatrix L = 1;
for(int j2=0; j2<deg_+1; ++j2){
if(j2 != j){
L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]);
}
}
SXFunction lfcn(tau,L);
lfcn.init();
// Evaluate the polynomial at the final time to get the coefficients of the continuity equation
lfcn.setInput(1.0);
lfcn.evaluate();
D[j] = lfcn.output();
D_num(j) = lfcn.output();
// Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
for(int j2=0; j2<deg_+1; ++j2){
lfcn.setInput(tau_root[j2]);
lfcn.setFwdSeed(1.0);
lfcn.evaluate(1,0);
C[j][j2] = lfcn.fwdSens();
C_num(j,j2) = lfcn.fwdSens();
}
}
C_num(std::vector<int>(1,0),ALL) = 0;
C_num(0,0) = 1;
// All collocation time points
vector<vector<double> > T(nk_);
for(int k=0; k<nk_; ++k){
T[k].resize(deg_+1);
for(int j=0; j<=deg_; ++j){
T[k][j] = h*(k + tau_root[j]);
}
}
// Total number of variables
int nlp_nx = 0;
nlp_nx += nk_*(deg_+1)*nx_; // Collocated states
nlp_nx += nk_*nu_; // Parametrized controls
nlp_nx += nx_; // Final state
// NLP variable vector
MX nlp_x = msym("x",nlp_nx);
int offset = 0;
// Get collocated states and parametrized control
vector<vector<MX> > X(nk_+1);
vector<MX> U(nk_);
for(int k=0; k<nk_; ++k){
// Collocated states
X[k].resize(deg_+1);
for(int j=0; j<=deg_; ++j){
// Get the expression for the state vector
X[k][j] = nlp_x[Slice(offset,offset+nx_)];
offset += nx_;
}
// Parametrized controls
U[k] = nlp_x[Slice(offset,offset+nu_)];
offset += nu_;
}
// State at end time
X[nk_].resize(1);
X[nk_][0] = nlp_x[Slice(offset,offset+nx_)];
offset += nx_;
casadi_assert(offset==nlp_nx);
// Constraint function for the NLP
vector<MX> nlp_g;
// Objective function
MX nlp_j = 0;
// For all finite elements
for(int k=0; k<nk_; ++k){
// For all collocation points
for(int j=1; j<=deg_; ++j){
// Get an expression for the state derivative at the collocation point
MX xp_jk = 0;
for(int r=0; r<=deg_; ++r){
xp_jk += C[r][j]*X[k][r];
}
// Add collocation equations to the NLP
MX fk = ffcn_.call(daeIn("x",X[k][j],"p",U[k]))[DAE_ODE];
nlp_g.push_back(h*fk - xp_jk);
}
// Get an expression for the state at the end of the finite element
MX xf_k = 0;
for(int r=0; r<=deg_; ++r){
xf_k += D[r]*X[k][r];
}
// Add continuity equation to NLP
nlp_g.push_back(X[k+1][0] - xf_k);
// Add path constraints
if(nh_>0){
MX pk = cfcn_.call(daeIn("x",X[k+1][0],"p",U[k])).at(0);
nlp_g.push_back(pk);
}
// Add integral objective function term
// [Jk] = lfcn.call([X[k+1,0], U[k]])
// nlp_j += Jk
}
// Add end cost
MX Jk = mfcn_.call(mayerIn("x",X[nk_][0])).at(0);
nlp_j += Jk;
// Objective function of the NLP
F_ = MXFunction(nlp_x, nlp_j);
// Nonlinear constraint function
G_ = MXFunction(nlp_x, vertcat(nlp_g));
// Get the NLP creator function
NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
// Allocate an NLP solver
nlp_solver_ = nlp_solver_creator(F_,G_,FX(),FX());
// Pass options
if(hasSetOption("nlp_solver_options")){
const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
nlp_solver_.setOption(nlp_solver_options);
}
// Initialize the solver
nlp_solver_.init();
}
static void
clutter_reflect_texture_paint (ClutterActor *actor)
{
ClutterReflectTexturePrivate *priv;
ClutterReflectTexture *texture;
ClutterClone *clone;
ClutterTexture *parent;
guint width, height;
gfloat fwidth, fheight;
gint r_height;
gint opacity;
gint bottom;
CoglHandle cogl_texture;
CoglTextureVertex tvert[4];
CoglFixed rty;
texture = CLUTTER_REFLECT_TEXTURE (actor);
clone = CLUTTER_CLONE (actor);
parent = (ClutterTexture*) clutter_clone_get_source (clone);
if (!parent)
return;
if (!CLUTTER_ACTOR_IS_REALIZED (parent))
clutter_actor_realize (CLUTTER_ACTOR (parent));
cogl_texture = clutter_texture_get_cogl_texture (parent);
if (cogl_texture == COGL_INVALID_HANDLE)
return;
priv = texture->priv;
clutter_actor_get_size (CLUTTER_ACTOR(parent), &fwidth, &fheight);
width = fwidth;
height = fheight;
if (!height)
// probably won't happen, but just in case, to avoid divide by zero.
return;
r_height = priv->reflection_height;
bottom = priv->reflect_bottom;
opacity = clutter_actor_get_opacity(actor);
if (r_height < 0 || r_height > height)
r_height = height;
#define FX(x) COGL_FIXED_FROM_INT(x)
rty = COGL_FIXED_FAST_DIV(FX(bottom ? height-r_height : r_height),FX(height));
/* clockise vertices and tex coords and colors! */
tvert[0].x = tvert[0].y = tvert[0].z = 0;
tvert[0].tx = 0; tvert[0].ty = bottom ? COGL_FIXED_1 : rty;
tvert[0].color.red = tvert[0].color.green = tvert[0].color.blue = 0xff;
tvert[0].color.alpha = bottom ? opacity : 0;
tvert[1].x = FX(width); tvert[1].y = tvert[1].z = 0;
tvert[1].tx = COGL_FIXED_1; tvert[1].ty = bottom ? COGL_FIXED_1 : rty;
tvert[1].color.red = tvert[1].color.green = tvert[1].color.blue = 0xff;
tvert[1].color.alpha = bottom ? opacity : 0;
tvert[2].x = FX(width); tvert[2].y = FX(r_height); tvert[2].z = 0;
tvert[2].tx = COGL_FIXED_1; tvert[2].ty = bottom ? rty : 0;
tvert[2].color.red = tvert[2].color.green = tvert[2].color.blue = 0xff;
tvert[2].color.alpha = bottom ? 0 : opacity;
tvert[3].x = 0; tvert[3].y = FX(r_height); tvert[3].z = 0;
tvert[3].tx = 0; tvert[3].ty = bottom ? rty : 0;
tvert[3].color.red = tvert[3].color.green = tvert[3].color.blue = 0xff;
tvert[3].color.alpha = bottom ? 0 : opacity;
cogl_push_matrix ();
cogl_set_source_texture(cogl_texture);
/* FIXME: this does not work as expected */
/* cogl_polygon(tvert, 4, TRUE); */
cogl_pop_matrix ();
}
FX OCPSolver::getRfcn() const { return isNull() ? FX(): (*this)->rfcn_; }
void
restore_mc (int enlarge_factor, image_t *image, const image_t *past,
const image_t *future, const wfa_t *wfa)
/*
* Restore motion compensated prediction of 'image' represented by 'wfa'.
* If 'enlarge_factor' != 0 then enlarge image by given amount.
* Reference frames are given by 'past' and 'future'.
*
* No return values.
*/
{
unsigned state, label;
unsigned root_state;
word_t *mcblock1, *mcblock2; /* MC blocks */
#define FX(v) ((image->format == FORMAT_4_2_0) && band != Y ? ((v) / 2) : v)
mcblock1 = Calloc (size_of_level (max ((int) wfa->wfainfo->p_max_level
+ 2 * enlarge_factor, 0)),
sizeof (word_t));
mcblock2 = Calloc (size_of_level (max ((int) wfa->wfainfo->p_max_level
+ 2 * enlarge_factor, 0)),
sizeof (word_t));
if (!image->color)
root_state = wfa->root_state;
else
root_state = wfa->tree [wfa->tree [wfa->root_state][0]][0];
for (state = wfa->basis_states; state <= root_state; state++)
for (label = 0; label < MAXLABELS; label++)
if (wfa->mv_tree[state][label].type != NONE)
{
color_e band;
unsigned level = wfa->level_of_state [state] - 1;
unsigned width = width_of_level (level);
unsigned height = height_of_level (level);
unsigned offset = image->width - width;
switch (wfa->mv_tree [state][label].type)
{
case FORWARD:
for (band = first_band (image->color);
band <= last_band (image->color); band++)
{
extract_mc_block (mcblock1, FX (width), FX (height),
past->pixels [band], FX (past->width),
wfa->wfainfo->half_pixel,
FX (wfa->x [state][label]),
FX (wfa->y [state][label]),
FX (wfa->mv_tree [state][label].fx),
FX (wfa->mv_tree [state][label].fy));
{
word_t *mc1; /* current pixel in MC block */
word_t *orig; /* current pixel in original image */
unsigned x, y; /* pixel coordinates */
mc1 = mcblock1;
orig = (word_t *) image->pixels [band]
+ FX (wfa->x[state][label])
+ FX (wfa->y[state][label]) * FX (image->width);
for (y = FX (height); y; y--)
{
for (x = FX (width); x; x--)
*orig++ += *mc1++;
orig += FX (offset);
}
}
}
break;
case BACKWARD:
for (band = first_band (image->color);
band <= last_band (image->color); band++)
{
extract_mc_block (mcblock1, FX (width), FX (height),
future->pixels [band],
FX (future->width),
wfa->wfainfo->half_pixel,
FX (wfa->x [state][label]),
FX (wfa->y [state][label]),
FX (wfa->mv_tree [state][label].bx),
FX (wfa->mv_tree [state][label].by));
{
word_t *mc1; /* current pixel in MC block 1 */
word_t *orig; /* current pixel in original image */
unsigned x, y; /* pixel coordinates */
mc1 = mcblock1;
orig = (word_t *) image->pixels [band]
+ FX (wfa->x[state][label])
+ FX (wfa->y[state][label]) * FX (image->width);
for (y = FX (height); y; y--)
{
for (x = FX (width); x; x--)
*orig++ += *mc1++;
orig += FX (offset);
}
}
}
break;
case INTERPOLATED:
for (band = first_band (image->color);
band <= last_band (image->color); band++)
{
extract_mc_block (mcblock1, FX (width), FX (height),
past->pixels [band], FX (past->width),
wfa->wfainfo->half_pixel,
FX (wfa->x[state][label]),
FX (wfa->y[state][label]),
FX (wfa->mv_tree[state][label].fx),
FX (wfa->mv_tree[state][label].fy));
extract_mc_block (mcblock2, FX (width), FX (height),
future->pixels [band],
FX (future->width),
wfa->wfainfo->half_pixel,
FX (wfa->x[state][label]),
FX (wfa->y[state][label]),
FX (wfa->mv_tree[state][label].bx),
FX (wfa->mv_tree[state][label].by));
{
word_t *mc1; /* current pixel in MC block 1 */
word_t *mc2; /* current pixel in MC block 1 */
word_t *orig; /* current pixel in original image */
unsigned x, y; /* pixel coordinates */
mc1 = mcblock1;
mc2 = mcblock2;
orig = (word_t *) image->pixels [band]
+ FX (wfa->x[state][label])
+ FX (wfa->y[state][label]) * FX (image->width);
for (y = FX (height); y; y--)
{
for (x = FX (width); x; x--)
#ifdef HAVE_SIGNED_SHIFT
*orig++ += (*mc1++ + *mc2++) >> 1;
#else /* not HAVE_SIGNED_SHIFT */
*orig++ += (*mc1++ + *mc2++) / 2;
#endif /* not HAVE_SIGNED_SHIFT */
orig += FX (offset);
}
}
}
break;
default:
break;
}
}
static cairo_status_t
twin_scaled_font_render_glyph (cairo_scaled_font_t *scaled_font,
unsigned long glyph,
cairo_t *cr,
cairo_text_extents_t *metrics)
{
double x1, y1, x2, y2, x3, y3;
const int8_t *b = _cairo_twin_outlines +
_cairo_twin_charmap[glyph >= ARRAY_LENGTH (_cairo_twin_charmap) ? 0 : glyph];
const int8_t *g = twin_glyph_draw(b);
struct {
cairo_bool_t snap;
int snap_x;
int snap_y;
int n_snap_x;
int n_snap_y;
} info = {FALSE};
cairo_set_line_width (cr, 0.06);
cairo_set_line_join (cr, CAIRO_LINE_JOIN_ROUND);
cairo_set_line_cap (cr, CAIRO_LINE_CAP_ROUND);
for (;;) {
switch (*g++) {
case 'M':
cairo_close_path (cr);
/* fall through */
case 'm':
x1 = FX(*g++);
y1 = FY(*g++);
if (info.snap)
{
x1 = SNAPX (x1);
y1 = SNAPY (y1);
}
cairo_move_to (cr, x1, y1);
continue;
case 'L':
cairo_close_path (cr);
/* fall through */
case 'l':
x1 = FX(*g++);
y1 = FY(*g++);
if (info.snap)
{
x1 = SNAPX (x1);
y1 = SNAPY (y1);
}
cairo_line_to (cr, x1, y1);
continue;
case 'C':
cairo_close_path (cr);
/* fall through */
case 'c':
x1 = FX(*g++);
y1 = FY(*g++);
x2 = FX(*g++);
y2 = FY(*g++);
x3 = FX(*g++);
y3 = FY(*g++);
if (info.snap)
{
x1 = SNAPX (x1);
y1 = SNAPY (y1);
x2 = SNAPX (x2);
y2 = SNAPY (y2);
x3 = SNAPX (x3);
y3 = SNAPY (y3);
}
cairo_curve_to (cr, x1, y1, x2, y2, x3, y3);
continue;
case 'E':
cairo_close_path (cr);
/* fall through */
case 'e':
cairo_stroke (cr);
break;
case 'X':
/* filler */
continue;
}
break;
}
metrics->x_advance = FX(twin_glyph_right(b)) + cairo_get_line_width (cr);
metrics->x_advance += cairo_get_line_width (cr)/* XXX 2*x.margin */;
if (info.snap)
metrics->x_advance = SNAPI (SNAPX (metrics->x_advance));
return CAIRO_STATUS_SUCCESS;
}
#endif
[_NAV] = TEMPLATE_NAV(
KC_GRV , XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX,
US_TAB , KC_EXLM, KC_AT , KC_HASH, KC_DLR , KC_PERC, KC_INS , KC_PGUP, KC_UP , KC_PGDN, KC_BTN1, KC_BTN3, KC_BTN2, KC_DEL ,
CTL_ESC, KC_LCBR, KC_RCBR, KC_LPRN, KC_RPRN, KC_AMPR, KC_HOME, KC_LEFT, KC_DOWN, KC_RGHT, KC_END , US_QUOT, TG_ADJ ,
KC_LSFT, KC_EQL, KC_PLUS, KC_MINS, KC_UNDS, KC_ASTR, KC_CALC, US_GRV , KC_WBAK, KC_WFWD, KC_WREF, KC_RSFT, KC_APP ,
KC_LCTL, KC_LGUI, KC_LALT, NV_SPC2, NV_SPC1, NV_SPC3, KC_RALT, KC_RGUI, KC_RCTL, KC_PTT ),
[_NUM] = TEMPLATE_NUM(
XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX, XXXXXXX,
KC_GRV , KC_F1 , KC_F2 , KC_F3 , KC_F4 , KC_VOLU, KC_CIRC, KC_7, KC_8, KC_9, KC_PMNS, XXXXXXX, XXXXXXX, KC_DEL ,
CTL_ESC, KC_F5 , KC_F6 , KC_F7 , KC_F8 , KC_MUTE, KC_PENT, KC_4, KC_5, KC_6, KC_PPLS, XXXXXXX, KC_ENT ,
KC_LSFT, KC_F9 , KC_F10 , KC_F11 , KC_F12 , KC_VOLD, KC_PIPE, KC_1, KC_2, KC_3, KC_PAST, KC_PSLS, TG_NUM ,
KC_LCTL, KC_LGUI, KC_LALT, NM_SPC2, NM_SPC1, NM_SPC3, KC_PDOT, KC_PCMM, KC_RCTL, KC_PTT ),
// Adjust layer is on the split-shift key; or NAV+Enter (for non-split keyboards)
[_ADJUST] = TEMPLATE_ADJUST(
MO_RST , FX(1) , FX(2) , FX(3) , FX(4) , FX(5) , FX(8) , FX(9) , FX(10) , FX(20) , FX(0) , BR_DEC , BR_INC , XXXXXXX, MO_RST ,
MO_RST , H1_INC , S1_INC , H2_INC , S2_INC , EF_INC , RGB_HUI, RGB_SAI, RGB_MOD, RGB_M_P, DFAULTS, RGB_VAD, RGB_VAI, MO_RST ,
XXXXXXX, H1_DEC , S1_DEC , H2_DEC , S2_DEC , EF_DEC , RGB_HUD, RGB_SAD, RGB_RMOD,RGB_M_K, RGB_M_B, RGB_M_G, TG_ADJ ,
TG_NKRO, LY_QWER, LY_WORK, LY_NRMN, LY_DVRK, LY_CLMK, XXXXXXX, LY_MALT, XXXXXXX, XXXXXXX, KC_MAKE, KC_CAPS, XXXXXXX,
MO_RST , AG_SWAP, AG_NORM, XXXXXXX, BL_TOGG, XXXXXXX, RGB_TOG, XXXXXXX, XXXXXXX, TG_GAME),
// To Reset hit FN + ` + Esc
[_RESET] = TEMPLATE_RESET,
};
void matrix_scan_user(void) {
#ifdef KEYBOARD_gh60
if (IS_LAYER_ON(_GAME)) {
gh60_wasd_leds_on();
} else {
gh60_wasd_leds_off();
}
// [[Rcpp::export]]
Rcpp::List fitData(Rcpp::DataFrame ds) {
const size_t ncoeffs = NCOEFFS;
const size_t nbreak = NBREAK;
const size_t n = N;
size_t i, j;
Rcpp::DataFrame D(ds); // construct the data.frame object
RcppGSL::vector<double> y = D["y"]; // access columns by name,
RcppGSL::vector<double> x = D["x"]; // assigning to GSL vectors
RcppGSL::vector<double> w = D["w"];
gsl_bspline_workspace *bw;
gsl_vector *B;
gsl_vector *c;
gsl_matrix *X, *cov;
gsl_multifit_linear_workspace *mw;
double chisq, Rsq, dof, tss;
bw = gsl_bspline_alloc(4, nbreak); // allocate a cubic bspline workspace (k = 4)
B = gsl_vector_alloc(ncoeffs);
X = gsl_matrix_alloc(n, ncoeffs);
c = gsl_vector_alloc(ncoeffs);
cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
mw = gsl_multifit_linear_alloc(n, ncoeffs);
gsl_bspline_knots_uniform(0.0, 15.0, bw); // use uniform breakpoints on [0, 15]
for (i = 0; i < n; ++i) { // construct the fit matrix X
double xi = gsl_vector_get(x, i);
gsl_bspline_eval(xi, B, bw); // compute B_j(xi) for all j
for (j = 0; j < ncoeffs; ++j) { // fill in row i of X
double Bj = gsl_vector_get(B, j);
gsl_matrix_set(X, i, j, Bj);
}
}
gsl_multifit_wlinear(X, w, y, c, cov, &chisq, mw); // do the fit
dof = n - ncoeffs;
tss = gsl_stats_wtss(w->data, 1, y->data, 1, y->size);
Rsq = 1.0 - chisq / tss;
Rcpp::NumericVector FX(151), FY(151); // output the smoothed curve
double xi, yi, yerr;
for (xi = 0.0, i=0; xi < 15.0; xi += 0.1, i++) {
gsl_bspline_eval(xi, B, bw);
gsl_multifit_linear_est(B, c, cov, &yi, &yerr);
FX[i] = xi;
FY[i] = yi;
}
Rcpp::List res =
Rcpp::List::create(Rcpp::Named("X")=FX,
Rcpp::Named("Y")=FY,
Rcpp::Named("chisqdof")=Rcpp::wrap(chisq/dof),
Rcpp::Named("rsq")=Rcpp::wrap(Rsq));
gsl_bspline_free(bw);
gsl_vector_free(B);
gsl_matrix_free(X);
gsl_vector_free(c);
gsl_matrix_free(cov);
gsl_multifit_linear_free(mw);
y.free();
x.free();
w.free();
return(res);
}
-1, X (o[0]), X (o[1]), X (o[2]), X (o[3]), X (o[4]), X (o[5]), X (o[6]), /* sp */ X (o[7]), /* ra */ -1, -1, -1, -1, -1, -1, -1, -1, /* l0 -> l7 */ -1, -1, -1, -1, -1, -1, -1, -1, /* i0 -> i7 */ FX (f.fregs[0]), /* f0 */ FX (f.fregs[1]), FX (f.fregs[2]), FX (f.fregs[3]), FX (f.fregs[4]), FX (f.fregs[5]), FX (f.fregs[6]), FX (f.fregs[7]), FX (f.fregs[8]), FX (f.fregs[9]), FX (f.fregs[10]), FX (f.fregs[11]), FX (f.fregs[12]), FX (f.fregs[13]), FX (f.fregs[14]), FX (f.fregs[15]),
static void flowgrad(float *y, float *flow, int w, int h)
{
float (*gradient)[w][4] = (void*)y;
getsample_operator p = getsample_1;
#define FX(i,j) p(flow,w,h,2,(i),(j),0)
#define FY(i,j) p(flow,w,h,2,(i),(j),1)
for (int j = 0; j < h; j++)
for (int i = 0; i < w; i++) {
// sobel
float ux = -FX(i-1,j-1)-2*FX(i-1,j)-FX(i-1,j+1)
+FX(i+1,j-1)+2*FX(i+1,j)+FX(i+1,j+1);
float uy = -FX(i-1,j-1)-2*FX(i,j-1)-FX(i+1,j-1)
+FX(i-1,j+1)+2*FX(i,j+1)+FX(i+1,j+1);
float vx = -FY(i-1,j-1)-2*FY(i-1,j)-FY(i-1,j+1)
+FY(i+1,j-1)+2*FY(i+1,j)+FY(i+1,j+1);
float vy = -FY(i-1,j-1)-2*FY(i,j-1)-FY(i+1,j-1)
+FY(i-1,j+1)+2*FY(i,j+1)+FY(i+1,j+1);
gradient[j][i][0] = ux;
gradient[j][i][1] = uy;
gradient[j][i][2] = vx;
gradient[j][i][3] = vy;
}
#undef FX
#undef FY
}
