void WeightedDerivativesToRefined::apply_index(Model *m,
ParticleIndex pi) const {
// retrieving pis by ref if possible is cumbersome but is required for speed
ParticleIndexes pis_if_not_byref;
ParticleIndexes const* pPis;
if(refiner_->get_is_by_ref_supported()){
ParticleIndexes const& pis =
refiner_->get_refined_indexes_by_ref(m, pi);
pPis = &pis;
} else{
pis_if_not_byref = refiner_->get_refined_indexes(m, pi);
pPis = &pis_if_not_byref;
}
ParticleIndexes const& pis = *pPis;
// Prepare derivative accumulator to normalize by total weight
Float total_weight;
if(w_ != FloatKey()){
total_weight = m->get_attribute(w_, pi);
} else {
total_weight = pis.size();
}
DerivativeAccumulator da( 1.0 / total_weight);
// read K values for each key in keys_
Floats Ks(keys_.size());
for (unsigned int j = 0; j < Ks.size(); ++j){
Ks[j] = m->get_derivative(keys_[j], pi);
}
// store K reweighted per each particle, normalized with da
for (unsigned int i = 0; i < pis.size(); ++i) {
Float w = m->get_attribute(w_, pis[i]);
for (unsigned int j = 0; j < keys_.size(); ++j) {
m->add_to_derivative(keys_[j], pis[i], w * Ks[j], da);
}
}
}
static PyObject *
_K_subscript(_K *self, PyObject *key)
{
int i;
K kobj = self->kobj;
char *skey;
int key_length;
int value_index = 1;
if (kobj->t != 5) {
PyErr_Format(PyExc_TypeError,
"k object of type %d is not a dictionary", kobj->t);
return NULL;
}
if (-1 == PyString_AsStringAndSize(key, &skey, &key_length)) {
return NULL;
}
if (skey[key_length-1] == '.') {
--key_length;
++value_index;
}
for (i=0; i < kobj->n; ++i) {
K e = KK(kobj)[i];
if (0 == strncmp(skey,Ks(KK(e)[0]),key_length)) {
PyTypeObject* type = self->ob_type;
_K* k = (_K*)type->tp_alloc(type, 0);
k->kobj = ci(KK(e)[value_index]);
return (PyObject*)k;
}
}
PyErr_SetObject(PyExc_KeyError, key);
return NULL;
}
void createMicrofacet30and0(BSDF* bsdf, bool beckmann) {
Spectrum Ks(0.5);
MicrofacetDistribution *distrib1, *distrib2;
if (beckmann) {
Float alphax = BeckmannDistribution::RoughnessToAlpha(0.8);
Float alphay = BeckmannDistribution::RoughnessToAlpha(0.8);
distrib1 = ARENA_ALLOC(arena, BeckmannDistribution)(alphax, alphay);
alphax = BeckmannDistribution::RoughnessToAlpha(0.01);
alphay = BeckmannDistribution::RoughnessToAlpha(0.01);
distrib2 = ARENA_ALLOC(arena, BeckmannDistribution)(alphax, alphay);
}
else {
Float alphax = TrowbridgeReitzDistribution::RoughnessToAlpha(0.8);
Float alphay = TrowbridgeReitzDistribution::RoughnessToAlpha(0.8);
distrib1 = ARENA_ALLOC(arena, TrowbridgeReitzDistribution)(alphax, alphay);
alphax = TrowbridgeReitzDistribution::RoughnessToAlpha(0.01);
alphay = TrowbridgeReitzDistribution::RoughnessToAlpha(0.01);
distrib2 = ARENA_ALLOC(arena, TrowbridgeReitzDistribution)(alphax, alphay);
}
Fresnel* fresnel = ARENA_ALLOC(arena, FresnelNoOp)();
BxDF* bxdf1 =
ARENA_ALLOC(arena, MicrofacetReflection)(Ks, distrib1, fresnel);
bsdf->Add(bxdf1);
BxDF* bxdf2 =
ARENA_ALLOC(arena, MicrofacetReflection)(Ks, distrib2, fresnel);
bsdf->Add(bxdf2);
}
K itemAtIndex(K a, I i) { // Return i-th item from any type as K - TODO: oom wherever this is used
I at=a->t;
if( 0< at)R ci(a);
if(-4==at)R Ks(kS(a)[i]); //could refactor all this
if(-3==at)R Kc(kC(a)[i]);
if(-2==at)R Kf(kF(a)[i]);
if(-1==at)R Ki(kI(a)[i]);
R ci(kK(a)[i]);
}
void createBlinn2(BSDF* bsdf)
{
const float blinnExponent = 2.0;
Spectrum Ks(1);
MicrofacetDistribution* distribution = BSDF_ALLOC(arena, Blinn)(blinnExponent);
Fresnel* fresnel = BSDF_ALLOC(arena, FresnelNoOp)();
BxDF* bxdf = BSDF_ALLOC(arena, Microfacet)(Ks, fresnel, distribution);
bsdf->Add(bxdf);
}
void createAniso30_30(BSDF* bsdf)
{
const float aniso1 = 30.0;
const float aniso2 = 30.0;
Spectrum Ks(1);
MicrofacetDistribution* distribution =
BSDF_ALLOC(arena, Anisotropic(aniso1, aniso2));
Fresnel* fresnel = BSDF_ALLOC(arena, FresnelNoOp)();
BxDF* bxdf = BSDF_ALLOC(arena, Microfacet)(Ks, fresnel, distribution);
bsdf->Add(bxdf);
}
void createBlinn30and0(BSDF* bsdf)
{
const float blinnExponent1 = 30.0;
Spectrum Ks(0.5);
MicrofacetDistribution* distribution1 = BSDF_ALLOC(arena, Blinn)(blinnExponent1);
Fresnel* fresnel = BSDF_ALLOC(arena, FresnelNoOp)();
BxDF* bxdf1 = BSDF_ALLOC(arena, Microfacet)(Ks, fresnel, distribution1);
bsdf->Add(bxdf1);
const float blinnExponent2 = 0.0;
MicrofacetDistribution* distribution2 = BSDF_ALLOC(arena, Blinn)(blinnExponent2);
BxDF* bxdf2 = BSDF_ALLOC(arena, Microfacet)(Ks, fresnel, distribution2);
bsdf->Add(bxdf2);
}
bool isRegistable(MapPoint* mp, FeaturePoint* new_fp, double pixelVar) {
if (mp->featPts.size() == 0)
return false;
size_t nview = mp->featPts.size();
Mat_d Ks(nview + 1, 9), Rs(nview + 1, 9), ts(nview + 1, 3), ms(nview + 1,
2), nms(nview + 1, 2);
double iK[9];
for (size_t c = 0; c < nview; c++) {
const FeaturePoint* fp = mp->featPts[c];
assert(fp->K && fp->cam);
const double* K = fp->K;
const double* R = fp->cam->R;
const double* t = fp->cam->t;
const double* m = fp->m;
doubleArrCopy(Ks, c, K, 9);
doubleArrCopy(Rs, c, R, 9);
doubleArrCopy(ts, c, t, 3);
doubleArrCopy(ms, c, m, 2);
getInvK(K, iK);
normPoint(iK, m, nms + 2 * c);
}
doubleArrCopy(Ks, nview, new_fp->K, 9);
doubleArrCopy(Rs, nview, new_fp->cam->R, 9);
doubleArrCopy(ts, nview, new_fp->cam->t, 3);
doubleArrCopy(ms, nview, new_fp->m, 2);
normPoint(iK, new_fp->m, nms + 2 * nview);
double M[3], m[2];
triangulateMultiView((int) (nview + 1), Rs, ts, nms, M);
//check the re-projection error
for (size_t c = 0; c < nview; c++) {
const FeaturePoint* fp = mp->featPts[c];
assert(fp->K && fp->cam);
project(fp->K, fp->cam->R, fp->cam->t, M, m);
if (dist2(m, fp->m) > pixelVar)
return false;
}
project(new_fp->K, new_fp->cam->R, new_fp->cam->t, M, m);
if (dist2(m, new_fp->m) > pixelVar)
return false;
return true;
}
static PyObject*
_get_Ks(PyObject* self, PyObject* ko)
{
K kobj;
int ok;
ok = ko && IS_K(ko);
if (!ok) goto fail;
kobj = ((_K*)ko)->kobj;
if (kobj && kobj->t == 4) {
char* s = Ks(kobj);
return Py_BuildValue("s", s);
}
fail:
PyErr_BadArgument();
return NULL;
}
static PyObject*
_set_Ks(PyObject* self, PyObject* args)
{
PyObject* ko;
char* s;
if (PyArg_ParseTuple(args, "O!s", &_KType, &ko, &s)) {
K k = ((_K*)ko)->kobj;
if (k && k->t == 4) {
Ks(k) = sp(s);
Py_INCREF(ko);
return ko;
} else {
PyErr_SetString(PyExc_TypeError, "wrong k type");
return NULL;
}
}
PyErr_BadArgument();
return NULL;
}
void createMicrofacet(BSDF* bsdf, MemoryArena& arena, bool beckmann,
bool samplevisible, float roughx, float roughy) {
Spectrum Ks(1);
MicrofacetDistribution* distrib;
if (beckmann) {
Float alphax = BeckmannDistribution::RoughnessToAlpha(roughx);
Float alphay = BeckmannDistribution::RoughnessToAlpha(roughy);
distrib = ARENA_ALLOC(arena, BeckmannDistribution)(alphax, alphay,
samplevisible);
} else {
Float alphax = TrowbridgeReitzDistribution::RoughnessToAlpha(roughx);
Float alphay = TrowbridgeReitzDistribution::RoughnessToAlpha(roughy);
distrib = ARENA_ALLOC(arena, TrowbridgeReitzDistribution)(
alphax, alphay, samplevisible);
}
Fresnel* fresnel = ARENA_ALLOC(arena, FresnelNoOp)();
BxDF* bxdf = ARENA_ALLOC(arena, MicrofacetReflection)(Ks, distrib, fresnel);
bsdf->Add(bxdf);
}
static int
_K_ass_sub(_K *self, PyObject *key, PyObject *value)
{
int i;
K kobj = self->kobj, kval;
char *skey;
int key_length;
int value_index = 1;
if (kobj->t != 5) {
PyErr_Format(PyExc_TypeError,
"k object of type %d is not a dictionary", kobj->t);
return -1;
}
if (-1 == PyString_AsStringAndSize(key, &skey, &key_length)) {
return -1;
}
if (value == NULL) {
PyErr_Format(PyExc_NotImplementedError, "k del key");
return -1;
}
if (_is_k(value)) {
kval = ((_K*)value)->kobj;
} else {
PyErr_Format(PyExc_TypeError, "value is not a k object");
return -1;
}
if (skey[key_length-1] == '.') {
--key_length;
++value_index;
}
for (i=0; i < kobj->n; ++i) {
K e = KK(kobj)[i];
if (0 == strncmp(skey,Ks(KK(e)[0]),key_length)) {
KK(e)[value_index] = ci(kval);
return 0;
}
}
PyErr_SetObject(PyExc_KeyError, key);
return -1;
}
void ForceConst::Svd(){
int n=Rms.dim1();
int StartLoop=static_cast<int> (Percent*n);
Array2D<double> U, V, S;
Array1D<double> s;
/*
for(int i=0;i<n;i++)
for(int j=0;j<n;j++) {
Rms[i][j]=Rms[i][j]*100.0;
if(Dist[i][j] > 0.6) {
cout << i << " " << j << " " << Dist[i][j] << endl;
Rms[i][j]=0.0;
}
}
*/
SVD<double> G(Rms);
G.getU(U);
G.getV(V);
G.getS(S);
G.getSingularValues(s);
Array2D<double> Sm1(n,n), Ks(n,n), Rms_b(n,n), V1(n,n), U1(n,n), Id(n,n);
for(int i=StartLoop;i<n;i++) S[i][i]=0.0;
V1=transpose(V);
U1=transpose(U);
Rms_b=matmult(U,matmult(S,V1));
Sm1=S;
for(int i=0;i<n;i++)
Sm1[i][i]=(Sm1[i][i] == 0.0)?0.0:1.0/Sm1[i][i];
Ks=matmult(V,matmult(Sm1,U1));
Id=matmult(transpose(Ks),Rms_b);
for(int i=0;i<n;i++) printf(" %5d %12.5e %12.5e \n",i,Rms[i][i],Rms_b[i][i]);
// for(int i=0;i<n;i++)
// for(int j=i;j<n;j++)
// printf(" %5d %5d %12.4f %12.5e %12.5e \n",i,j,Dist[i][j],Ks[i][j], Rms[i][j]);
}
void updatePointUncertainty(MapPoint* mp, FeaturePoint* new_fp,
double pixelVar) {
if (mp->featPts.size() == 0)
return;
size_t nview = mp->featPts.size();
Mat_d Ks(nview + 1, 9), Rs(nview + 1, 9), ts(nview + 1, 3), ms(nview + 1,
2), nms(nview + 1, 2);
double iK[9];
for (size_t c = 0; c < nview; c++) {
const FeaturePoint* fp = mp->featPts[c];
assert(fp->K && fp->cam);
const double* K = fp->K;
const double* R = fp->cam->R;
const double* t = fp->cam->t;
const double* m = fp->m;
doubleArrCopy(Ks, c, K, 9);
doubleArrCopy(Rs, c, R, 9);
doubleArrCopy(ts, c, t, 3);
doubleArrCopy(ms, c, m, 2);
getInvK(K, iK);
normPoint(iK, m, nms + 2 * c);
}
doubleArrCopy(Ks, nview, new_fp->K, 9);
doubleArrCopy(Rs, nview, new_fp->cam->R, 9);
doubleArrCopy(ts, nview, new_fp->cam->t, 3);
doubleArrCopy(ms, nview, new_fp->m, 2);
normPoint(iK, new_fp->m, nms + 2 * nview);
getTriangulateCovMat((int) (nview + 1), Ks, Rs, ts, mp->M, mp->cov,
pixelVar);
}
/* XXX unfortunately API function gnk of which pyk.gk
is based is a vararg function and therefore cannot
be portably exported to Python. It would be better
if libk20 supplied a function gnk_(I, K*)
in addition to gnk(I,...) which would take an array
of K objects as the second argument */
static PyObject*
_gk(PyObject* self, PyObject* args)
{
int n = PyTuple_Size(args);
if (!n) {
return _mk_K(gtn(0,0));
}
int i, type = INT_MAX;
K* ks = (K*)malloc(n*sizeof(K));
K kobj;
for(i = 0; i < n; i++) {
K ki;
int t;
PyObject* argi = PyTuple_GET_ITEM(args, i);
if (!IS_K(argi)) {
goto fail;
}
ks[i] = ki = ((_K*)argi)->kobj;
t = ki->t;
if (INT_MAX == type) {
type = t;
} else if (t > 4 || t < 1 || t != type) {
type = 0;
}
}
kobj = gtn((type>0 && type<5)?-type:0, n);
if (!kobj) {
free(ks);
return PyErr_Format(PyExc_TypeError, "gtn(%d,%d) returned null", -type, n);
}
switch (type) {
case 1:
for (i = 0; i < n; i++) {
KI(kobj)[i] = Ki(ks[i]);
}
break;
case 2:
for (i = 0; i < n; i++) {
KF(kobj)[i] = Kf(ks[i]);
}
break;
case 3:
for (i = 0; i < n; i++) {
KC(kobj)[i] = Kc(ks[i]);
}
break;
case 4:
for (i = 0; i < n; i++) {
KS(kobj)[i] = Ks(ks[i]);
}
break;
default:
memcpy(KK(kobj), ks, n*sizeof(K));
for (i = 0; i < n; i++) {
ci(ks[i]);
}
break;
}
free(ks);
return _mk_K(kobj);
fail:
free(ks);
PyErr_BadArgument();
return NULL;
}
//! computes interaction with a ground surface
bool simulation::AerodynamicModelContact::compute(
AerodynamicForce &aeroForce,
AerodynamicModelContactState &dynamicState,
const AerodynamicModelContactControl &control,
const dynamics::RigidBody &rigidBody,
const GroundSurface &surface) {
math::matrix3x1<float> contactLocation = rigidBody.location +
rigidBody.orientation.rotate(location);
math::matrix3x1<float> contactVelocity =
rigidBody.localParticleVelocity(location);
aeroForce.lift = math::matrix3x1<float>(0, 0, 0);
aeroForce.drag = math::matrix3x1<float>(0, 0, 0);
aeroForce.location = contactLocation;
aeroForce.velocity = contactVelocity;
aeroForce.type = AerodynamicForce::Type_invalid;
// first detect if there is a contact, defined as the location of the contact on the opposite
// side of the surface as the location of the rigidBody center
math::matrix3x1<float> P = surface.surface.normal() +
surface.surface.normal() * surface.surface.intersect(math::matrix3x1<float>::zero(), surface.surface.normal());
if (signum(surface.surface(contactLocation)) != signum(surface.surface(P))) {
math::matrix3x1<float> surfaceNorm = surface.surface.normal();
float t_i = surface.surface.intersect(contactLocation, surfaceNorm);
math::matrix3x1<float> intersection = t_i * surfaceNorm + contactLocation;
// project displacement and velocity
float t_i_dot = surfaceNorm.dot(contactVelocity);
if (impulseType == AerodynamicContactResponse::Force_impulse) {
if (t_i_dot < 0) {
math::quaternion<float> orientation = rigidBody.orientation * control.orientation;
// compute dashpot force
float forceMagnitude = Ks() * t_i - Kd() * t_i_dot;
math::matrix3x1<float> normalForce = surfaceNorm * forceMagnitude;
dynamicState.load = forceMagnitude;
// compute friction
math::matrix3x1<float> tangentVelocity = surface.surface.project(contactVelocity);
float frictionRolling = math::lerp<float>(control.brake, friction.rollingCoeff(), friction.dynamicCoeff()) * forceMagnitude;
float frictionSkidding = friction.dynamicCoeff() * forceMagnitude;
math::matrix3x1<float> tangentForce(0, 0, 0);
if (tangentVelocity.normsq() > 0.0001f) {
math::matrix3x1<float> V = tangentVelocity.unit();
math::matrix3x1<float> F = surface.surface.project(orientation.rotate(math::matrix3x1<float>(0,0,1))).unit();
math::matrix3x1<float> S = tangentVelocity.cross(surfaceNorm);
math::matrix3x1<float> rollingFriction = -frictionRolling * (std::abs(V.dot(F))) * V;
math::matrix3x1<float> skiddingFriction = frictionSkidding * (S.dot(F)) * S.unit();
tangentForce = rollingFriction + skiddingFriction;
}
else {
}
// wheels spinning
dynamicState.angularVelocity = -tangentVelocity.magnitude() / bogeyModel.x;
// compute netforce
aeroForce.lift = normalForce;
aeroForce.drag = tangentForce;
aeroForce.location = rigidBody.orientation.rotate(location);
aeroForce.type = AerodynamicForce::Contact_force;
return true;
}
}
else if (type == AerodynamicContactResponse::Velocity_impulse) {
// zero the velocity
}
}
else {
dynamicState.load = 0;
}
return false;
}
K gs(S x) {K z=newK(4,1); Ks(z)=x; R z;}
/* The computational routine */
void gradOrder1ScaleC(double *dy, double *yt, double *rhot, int L, int R, int dim, int CSP, int CSD, double *scales2, double *scaleweight2, double energyweight)
{
/* dy already initialized to zeros */
int CSPL = CSP*L;
int CSDL = 1;
double invR = 1.0/R;
int i, l, sl, si;
if (dim == 2) {
#pragma omp parallel for schedule(static) shared(dy,yt,rhot,L,R,dim,CSP,CSD,CSPL,CSDL,scales2,scaleweight2) private(i,l,sl,si)
for (i=0; i<L; i++) { /* particle */
double xi[2] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)]};
double dxi[2] = {yt[INDYX(i,0,0,0,0)],yt[INDYX(i,1,0,0,0)]};
for (l=0; l<L; l++) { /* particle */
double xl[2] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)]};
double dxl[2] = {yt[INDYX(l,0,0,0,0)],yt[INDYX(l,1,0,0,0)]};
double ximxl[2]; VECMINUS(ximxl,xi,xl);
double v = VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[2] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)]};
double aisl[2] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double d1ksl = D1Ks(kbasesl,rsl2,swsl2);
double d1kslXximxl[2]; VECSCALAR(d1kslXximxl,d1ksl,ximxl);
double dalsl[2] = {yt[INDYP(l,0,sl,0,0,0)],yt[INDYP(l,1,sl,0,0,0)]};
/* dx */
double rt1; VECDOT(rt1,aisl,dxl);
double rt2; VECDOT(rt2,alsl,dxi);
double vt1[2]; VECSCALAR(vt1,2*(rt1+rt2),d1kslXximxl);
dy[INDYX(i,0,0,0,0)] += vt1[0];
dy[INDYX(i,1,0,0,0)] += vt1[1];
for (si=0; si<R; si++) { /* scale */
double alsi[2] = {rhot[INDRHOP(l,0,si)],rhot[INDRHOP(l,1,si)]};
double aisi[2] = {rhot[INDRHOP(i,0,si)],rhot[INDRHOP(i,1,si)]};
double daisi[2] = {yt[INDYP(i,0,si,0,0,0)],yt[INDYP(i,1,si,0,0,0)]};
double daisl[2] = {yt[INDYP(i,0,sl,0,0,0)],yt[INDYP(i,1,sl,0,0,0)]};
double rsi2 = scales2[si];
double swsi2 = scaleweight2[si];
double kbasesi = exp(-v/rsi2);
double ksi = Ks(kbasesi,swsi2);
double d1ksi = D1Ks(kbasesi,rsi2,swsi2);
double d2ksi = D2Ks(kbasesi,rsi2,swsi2);
double aislTXalsi; VECDOT(aislTXalsi,aisl,alsi);
double aisiTXalsl; VECDOT(aisiTXalsl,aisi,alsl);
/* dx */
VECSCALAR(vt1,aislTXalsi,daisl);
double vt2[2]; VECSCALAR(vt2,aisiTXalsl,dalsl);
double vt3[2]; VECMINUS(vt3,vt1,vt2);
double rt; VECDOT(rt,ximxl,vt3);
VECSCALAR(vt1,-2*d1ksi,vt3);
VECSCALAR(vt2,-4*d2ksi*rt,ximxl);
VECADD(vt3,vt1,vt2);
dy[INDYX(i,0,0,0,0)] += vt3[0];
dy[INDYX(i,1,0,0,0)] += vt3[1];
/* da */
VECSCALAR(vt1,d1ksl,daisi);
VECSCALAR(vt2,d1ksi,dalsl);
VECMINUS(vt3,vt1,vt2);
VECDOT(rt,ximxl,vt3);
VECSCALAR(vt1,-2*rt,alsl);
VECSCALAR(vt2,invR*ksi,dxl);
VECADD(vt3,vt1,vt2);
dy[INDYP(i,0,si,0,0,0)] += vt3[0];
dy[INDYP(i,1,si,0,0,0)] += vt3[1];
}
}
}
}
} else {
/* #pragma omp parallel for schedule(static) shared(dy,yt,rhot,L,R,dim,CSP,CSD,CSPL,CSDL,scales2,scaleweight2) private(i,l,sl,si)*/
for (i=0; i<L; i++) { /* particle */
double xi[3] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)],rhot[INDRHOX(i,2)]};
double dxi[3] = {yt[INDYX(i,0,0,0,0)],yt[INDYX(i,1,0,0,0)],yt[INDYX(i,2,0,0,0)]};
for (l=0; l<L; l++) { /* particle */
double xl[3] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)],rhot[INDRHOX(l,2)]};
double dxl[3] = {yt[INDYX(l,0,0,0,0)],yt[INDYX(l,1,0,0,0)],yt[INDYX(l,2,0,0,0)]};
double ximxl[3]; _3VECMINUS(ximxl,xi,xl);
double v = _3VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[3] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)],rhot[INDRHOP(l,2,sl)]};
double aisl[3] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)],rhot[INDRHOP(i,2,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double d1ksl = D1Ks(kbasesl,rsl2,swsl2);
double d1kslXximxl[3]; _3VECSCALAR(d1kslXximxl,d1ksl,ximxl);
double dalsl[3] = {yt[INDYP(l,0,sl,0,0,0)],yt[INDYP(l,1,sl,0,0,0)],yt[INDYP(l,2,sl,0,0,0)]};
/* dx */
double rt1; _3VECDOT(rt1,aisl,dxl);
double rt2; _3VECDOT(rt2,alsl,dxi);
double vt1[3]; _3VECSCALAR(vt1,2*(rt1+rt2),d1kslXximxl);
dy[INDYX(i,0,0,0,0)] += vt1[0];
dy[INDYX(i,1,0,0,0)] += vt1[1];
dy[INDYX(i,2,0,0,0)] += vt1[2];
for (si=0; si<R; si++) { /* scale */
double alsi[3] = {rhot[INDRHOP(l,0,si)],rhot[INDRHOP(l,1,si)],rhot[INDRHOP(l,2,si)]};
double aisi[3] = {rhot[INDRHOP(i,0,si)],rhot[INDRHOP(i,1,si)],rhot[INDRHOP(i,2,si)]};
double daisi[3] = {yt[INDYP(i,0,si,0,0,0)],yt[INDYP(i,1,si,0,0,0)],yt[INDYP(i,2,si,0,0,0)]};
double daisl[3] = {yt[INDYP(i,0,sl,0,0,0)],yt[INDYP(i,1,sl,0,0,0)],yt[INDYP(i,2,sl,0,0,0)]};
double rsi2 = scales2[si];
double swsi2 = scaleweight2[si];
double kbasesi = exp(-v/rsi2);
double ksi = Ks(kbasesi,swsi2);
double d1ksi = D1Ks(kbasesi,rsi2,swsi2);
double d2ksi = D2Ks(kbasesi,rsi2,swsi2);
double aislTXalsi; _3VECDOT(aislTXalsi,aisl,alsi);
double aisiTXalsl; _3VECDOT(aisiTXalsl,aisi,alsl);
/* dx */
_3VECSCALAR(vt1,aislTXalsi,daisl);
double vt2[3]; _3VECSCALAR(vt2,aisiTXalsl,dalsl);
double vt3[3]; _3VECMINUS(vt3,vt1,vt2);
double rt; _3VECDOT(rt,ximxl,vt3);
_3VECSCALAR(vt1,-2*d1ksi,vt3);
_3VECSCALAR(vt2,-4*d2ksi*rt,ximxl);
_3VECADD(vt3,vt1,vt2);
dy[INDYX(i,0,0,0,0)] += vt3[0];
dy[INDYX(i,1,0,0,0)] += vt3[1];
dy[INDYX(i,2,0,0,0)] += vt3[2];
/* da */
_3VECSCALAR(vt1,d1ksl,daisi);
_3VECSCALAR(vt2,d1ksi,dalsl);
_3VECMINUS(vt3,vt1,vt2);
_3VECDOT(rt,ximxl,vt3);
_3VECSCALAR(vt1,-2*rt,alsl);
_3VECSCALAR(vt2,invR*ksi,dxl);
_3VECADD(vt3,vt1,vt2);
dy[INDYP(i,0,si,0,0,0)] += vt3[0];
dy[INDYP(i,1,si,0,0,0)] += vt3[1];
dy[INDYP(i,2,si,0,0,0)] += vt3[2];
}
}
}
}
}
addEgradt(dy,yt,rhot,L,R,dim,CSP,CSD,scales2,scaleweight2,energyweight);
}
/* energy gradient (helper function) */
void addEgradt(double *dy, double *yt, double *rhot, int L, int R, int dim, int CSP, int CSD, double *scales2, double *scaleweight2, double energyweight)
{
int CSPL = CSP*L;
int CSDL = 1;
int i, l, sl, si;
if (dim == 2) {
#pragma omp parallel for schedule(static) shared(dy,yt,rhot,L,R,dim,CSP,CSD,CSPL,CSDL,scales2,scaleweight2,energyweight) private(i,l,sl,si)
for (i=0; i<L; i++) { /* particle */
double xi[2] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)]};
/* debug */
/*mexPrintf("xi %f %f\n",xi[0],xi[1]);*/
for (l=0; l<L; l++) { /* particle */
double xl[2] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)]};
/* debug */
/*mexPrintf("xl %f %f\n",xl[0],xl[1]);*/
double ximxl[2]; VECMINUS(ximxl,xi,xl);
double v = VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[2] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)]};
double aisl[2] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double ksl = Ks(kbasesl,swsl2);
double d1ksl = D1Ks(kbasesl,rsl2,swsl2);
double d1kslXximxl[2]; VECSCALAR(d1kslXximxl,d1ksl,ximxl);
double d2ksl = D2Ks(kbasesl,rsl2,swsl2);
double aislTXalsl; VECDOT(aislTXalsl,aisl,alsl);
/* dx */
double vt1[2]; VECSCALAR(vt1,energyweight*4*d1ksl*aislTXalsl,ximxl);
dy[INDYX(i,0,0,0,0)] += vt1[0];
dy[INDYX(i,1,0,0,0)] += vt1[1];
/* da */
VECSCALAR(vt1,energyweight*2*ksl,alsl);
dy[INDYP(i,0,sl,0,0,0)] += vt1[0];
dy[INDYP(i,1,sl,0,0,0)] += vt1[1];
}
}
}
} else {
#pragma omp parallel for schedule(static) shared(dy,yt,rhot,L,R,dim,CSP,CSD,CSPL,CSDL,scales2,scaleweight2,energyweight) private(i,l,sl,si)
for (i=0; i<L; i++) { /* particle */
double xi[3] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)],rhot[INDRHOX(i,2)]};
for (l=0; l<L; l++) { /* particle */
double xl[3] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)],rhot[INDRHOX(l,2)]};
double ximxl[3]; _3VECMINUS(ximxl,xi,xl);
double v = _3VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[3] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)],rhot[INDRHOP(l,2,sl)]};
double aisl[3] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)],rhot[INDRHOP(i,2,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double ksl = Ks(kbasesl,swsl2);
double d1ksl = D1Ks(kbasesl,rsl2,swsl2);
double d1kslXximxl[3]; _3VECSCALAR(d1kslXximxl,d1ksl,ximxl);
double d2ksl = D2Ks(kbasesl,rsl2,swsl2);
double aislTXalsl; _3VECDOT(aislTXalsl,aisl,alsl);
/* dx */
double vt1[3]; _3VECSCALAR(vt1,energyweight*4*d1ksl*aislTXalsl,ximxl);
dy[INDYX(i,0,0,0,0)] += vt1[0];
dy[INDYX(i,1,0,0,0)] += vt1[1];
dy[INDYX(i,2,0,0,0)] += vt1[2];
/* da */
_3VECSCALAR(vt1,energyweight*2*ksl,alsl);
dy[INDYP(i,0,sl,0,0,0)] += vt1[0];
dy[INDYP(i,1,sl,0,0,0)] += vt1[1];
dy[INDYP(i,2,sl,0,0,0)] += vt1[2];
}
}
}
}
}
void CStateAuthentication::ProcessDataL(const TDesC8& aData)
{
//free resources
delete iRequestData;
iRequestData = NULL;
if(aData.Size()!=0)
{
if(iResponseData == NULL)
{
iResponseData = aData.AllocL();
}
else
{
TInt size = iResponseData->Size();
iResponseData = iResponseData->ReAllocL(size+aData.Size()); //TODO:check this
iResponseData->Des().Append(aData);
}
return;
}
// parse response from server
if(iResponseData->Find(KApplicationOS)==KErrNotFound)
{
//server answered with a redirect
iObserver.ResponseError(KErrAuth);
return;
}
//retrieve cookie
HBufC8* cookie = CRestUtils::GetCookieL(*iResponseData);
TBuf8<32> cookieBuf(*cookie);
delete cookie;
iObserver.SetCookie(cookieBuf);
//extract body from response
RBuf8 body(CRestUtils::GetBodyL(*iResponseData));
body.CleanupClosePushL();
// first 32 bytes are Crypt(Signature,Ks)
TPtrC8 crypt2 = body.Left(32);
// retrieve Ks
RBuf8 Ks(AES::DecryptPkcs5L(crypt2,KIV,iSignKey));
Ks.CleanupClosePushL();
// then calculate K=sha1(confKey,Ks,Kd), take only first 16 bytes of sha1
CSHA1* sha1 = CSHA1::NewL();
CleanupStack::PushL(sha1);
sha1->Update(iConfKey);
sha1->Update(Ks);
TBuf8<20> keyK;
keyK.Copy(sha1->Final(iKd));
CleanupStack::PopAndDestroy(sha1);
keyK.SetLength(16);
CleanupStack::PopAndDestroy(&Ks);
//set K key
iObserver.SetKey(keyK);
// last bytes are Crypt(K,Nonce | Response)
TPtrC8 crypt3 = body.Right(body.Size()-32);
RBuf8 plain(AES::DecryptPkcs5L(crypt3,KIV,keyK));
plain.CleanupClosePushL();
TBufC8<16> nonce(plain.Left(16));
TBufC8<4> response(plain.Right(4));
CleanupStack::PopAndDestroy(&plain);
CleanupStack::PopAndDestroy(&body);
//verify nonce
if(iNonce.Compare(nonce)!=0)
{
iObserver.ResponseError(KErrAuth);
return;
}
//verify response
if(response.Compare(KProto_Ok)==0)
{
// it's ok, let's go on
iObserver.ChangeStateL();
return;
}
if(response.Compare(KProto_No)==0)
{
iObserver.ResponseError(KErrAuth);
return;
}
if(response.Compare(KProto_CmdUninstall)==0)
{
CAbstractState* uninstall = CStateCmdUninstall::NewL(iObserver);
uninstall->ActivateL(KNullDesC8);
return;
}
}
K glue(K a, K b) { R Ks(sp(glueSS(*kS(a),*kS(b)))); } //oom
int
main(int argc, char** argv)
{
F pi = atan(1.0)*4;
K a = gi(2);
K b = gi(3);
K c = gi(4);
K* v;
cd(ksk("",0));
tst(Ki(a)==2);
tst(Ki(b) + 1 == Ki(c));
cd(a); cd(b); cd(c);
b = gf(1.0); c = gf(2);
tst(Kf(b) + 1 == Kf(c));
cd(b); cd(c);
a = gs(sp("foo"));
b = ksk("`foo", 0);
tst(Ks(a) == Ks(b));
cd(a); cd(b);
a = ksk("2 + 3", 0);
tst(Ki(a) == 5);
cd(a);
a = ksk("_ci 65", 0);
tst(Kc(a) == 'A');
// XXX this should return type 1 uniform vector
a=gnk(3,gi(11),gi(22),gi(33));
tst(a->t == 0);
v = (K*)a->k;
tst(Ki(v[0])+Ki(v[1])==Ki(v[2]));
cd(a);
{
b = gsk("pi",gf(pi));
kap(&KTREE, &b);
a = X(".pi");
tst(Kf(a) == pi);
cd(a);
}
{
K dir = gtn(5,0);
K t;
t = gsk("x",gi(1)); kap(&dir, &t);
t = gsk("y",gi(2)); kap(&dir, &t);
t = gsk("z",dir); kap(&KTREE, &t);
a = X(".z.x");
tst(Ki(a) == 1);
cd(a);
a = X(".z.y");
tst(Ki(a) == 2);
cd(a);
}
{
I i;
K d = gtn(5,0);
K c0 = gtn(0,0);
K c1 = gtn(-1,0);
K t0, t1, e;
t0 = gsk("a", c0); kap(&d,&t0);
t1 = gsk("b", c1); kap(&d,&t1);
e = gp("hello1"); kap(&c0,&e);
e = gp("hello2"); kap(&c0,&e);
KK(KK(d)[0])[1] = c0;
i = 1; kap(&KK(KK(d)[1])[1], &i);
i = 2; kap(&KK(KK(d)[1])[1], &i);
//i = 1; kap(&c1, &i);
//i = 2; kap(&c1, &i);
//KK(KK(d)[1])[1] = c1;
show(d);
}
//b = ksk("+/", a);
//tst(Ki(b) == 66);
//argc--;argv++;
//DO(i, argc, {a=ksk(argv[i], 0);
//ksk("`0:,/$!10;`0:,\"\n\"", 0);
fprintf(stderr, "Pass:%4d, fail:%4d\n", pass, fail);
if (argc > 1 && strcmp(argv[1], "-i") == 0) {
boilerplate();
attend();
}
}
/* The computational routine */
double energyScaleC(double *Et, double *rhot, int L, int R, int dim, int CSP, double *scales2, double *scaleweight2)
{
/* Et already initialized to zeros */
int CSPL = CSP*L;
int i, l, sl;
double Ett = 0;
if (dim == 2) {
#pragma omp parallel for schedule(static) shared(rhot,L,R,dim,CSP,CSPL,scales2,scaleweight2) private(i,l,sl) reduction(+:Ett)
for (i=0; i<L; i++) { /* particle */
double xi[2] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)]};
for (l=0; l<L; l++) { /* particle */
double xl[2] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)]};
double ximxl[2]; VECMINUS(ximxl,xi,xl);
double v = VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[2] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)]};
double aisl[2] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double ksl = Ks(kbasesl,swsl2);
double aislTXalsi; VECDOT(aislTXalsi,aisl,alsl);
Ett = Ett+ksl*aislTXalsi;
}
}
}
} else {
#pragma omp parallel for schedule(static) shared(rhot,L,R,dim,CSP,CSPL,scales2,scaleweight2) private(i,l,sl) reduction(+:Ett)
for (i=0; i<L; i++) { /* particle */
double xi[3] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)],rhot[INDRHOX(i,2)]};
for (l=0; l<L; l++) { /* particle */
double xl[3] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)],rhot[INDRHOX(l,2)]};
double ximxl[3]; _3VECMINUS(ximxl,xi,xl);
double v = _3VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[3] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)],rhot[INDRHOP(l,2,sl)]};
double aisl[3] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)],rhot[INDRHOP(i,2,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double ksl = Ks(kbasesl,swsl2);
double aislTXalsi; _3VECDOT(aislTXalsi,aisl,alsl);
Ett = Ett+ksl*aislTXalsi;
}
}
}
}
Et[0] = Ett;
}
static PyObject*
_ktoarray(PyObject* self, PyObject* k)
{
PyArrayObject *ret;
if (!PyK_KCheck(k)) {
return PyErr_Format(PyExc_TypeError, "not k object");
}
K kobj = ((PyK_K*)k)->kobj;
if (!kobj) {
return PyErr_Format(PyExc_AssertionError, "null kobj");
}
int t = kobj->t;
/* XXX k objects of type 0 should be converted
* to non-contiguous arrays rather than trigger
* an error.
*/
if (abs(t) >= LEN(types) && t != 5 ) {
return PyErr_Format(PyExc_TypeError,
"cannot create an array from a "
"k object of type %d", t);
}
int type = types[abs(t)]; /* PyArray type */
int nd = t <= 0 || t == 5; /* Number of dimensions (0 or 1) */
int* d = &kobj->n; /* Shape */
char* data;
switch (t) {
case 1:
data = (char*)&Ki(kobj);
break;
case 3:
data = &Kc(kobj);
break;
case 4:
data = Ks(kobj);
break;
default:
data = KC(kobj);
}
/* Special handling for symbols arrays: convert data to Python strings */
PyObject** buf = 0;
if (t == -4) {
int n = *d, i = 0;
buf = (PyObject**)malloc(n * sizeof(PyObject*));
for (i = 0; i < n; ++i) {
char* s = KS(kobj)[i];
if (!s) goto fail;
buf[i] = PyString_FromString(s);
if (!buf[i]) goto fail;
}
data = (char*)buf;
} else if (t == 0 || t == 5) {
int n = *d, i = 0;
buf = (PyObject**)malloc(n * sizeof(PyObject*));
for (i = 0; i < n; ++i) {
K ki = KK(kobj)[i];
if (!ki) goto fail;
ci(ki);
buf[i] = PyK_mk_K(ki);
if (!buf[i]) {
cd(ki);
goto fail;
}
}
data = (char*)buf;
}
if (!(ret = (PyArrayObject *)PyArray_FromDimsAndData(nd, d, type, data))) {
goto fail;
}
if (buf) {
ret->flags |= OWN_DATA;
} else {
Py_INCREF(k);
ret->base = k;
}
return (PyObject*)ret;
fail:
if (buf) free(buf);
return NULL;
}
