float Noise::smoothNoise2(Vec2f &vec)
{
int bx0, bx1, by0, by1, b00, b10, b01, b11;
float rx0, rx1, ry0, ry1, *q, sx, sy, a, b, t, u, v;
int i, j;
SETUP(vec.x(), bx0, bx1, rx0, rx1);
SETUP(vec.y(), by0, by1, ry0, ry1);
i = p[bx0];
j = p[bx1];
b00 = p[i + by0];
b10 = p[j + by0];
b01 = p[i + by1];
b11 = p[j + by1];
sx = SCURVE(rx0);
sy = SCURVE(ry0);
#define AT2(rx, ry) ((rx)*q[0] + (ry)*q[1])
q = g2[b00];
u = AT2(rx0, ry0);
q = g2[b10];
v = AT2(rx1, ry0);
a = LERP(sx, u, v);
q = g2[b01];
u = AT2(rx0, ry1);
q = g2[b11];
v = AT2(rx1, ry1);
b = LERP(sx, u, v);
#undef AT2
return LERP(sy, a, b);
}
/* Returns a satTypeValueMap object, adding the new data generated when
* calling this object.
*
* @param time Epoch corresponding to the data.
* @param gData Data object holding the data.
*/
satTypeValueMap& ComputeDOP::Process( const CommonTime& time,
satTypeValueMap& gData)
throw(ProcessingException)
{
try
{
bool valid1(false), valid2(false);
// First, let's define a set with XYZt unknowns
TypeIDSet tempSet1;
tempSet1.insert(TypeID::dx);
tempSet1.insert(TypeID::dy);
tempSet1.insert(TypeID::dz);
tempSet1.insert(TypeID::cdt);
// Second, let's define a set with NEUt unknowns
TypeIDSet tempSet2;
tempSet2.insert(TypeID::dLat);
tempSet2.insert(TypeID::dLon);
tempSet2.insert(TypeID::dH);
tempSet2.insert(TypeID::cdt);
// Then, generate the corresponding geometry/design matrices
Matrix<double> dMatrix1(gData.getMatrixOfTypes(tempSet1));
Matrix<double> dMatrix2(gData.getMatrixOfTypes(tempSet2));
// Afterwards, compute the appropriate extra matrices
Matrix<double> AT1(transpose(dMatrix1));
Matrix<double> covM1(AT1 * dMatrix1);
Matrix<double> AT2(transpose(dMatrix2));
Matrix<double> covM2(AT2 * dMatrix2);
// Let's try to invert AT*A matrices
try
{
covM1 = inverseChol( covM1 );
valid1 = true;
}
catch(...)
{
valid1 = false;
}
try
{
covM2 = inverseChol( covM2 );
valid2 = true;
}
catch(...)
{
valid2 = false;
}
if( valid1 )
{
gdop = std::sqrt(covM1(0,0)+covM1(1,1)+covM1(2,2)+covM1(3,3));
pdop = std::sqrt(covM1(0,0)+covM1(1,1)+covM1(2,2));
tdop = std::sqrt(covM1(3,3));
}
else
{
gdop = -1.0;
pdop = -1.0;
tdop = -1.0;
}
if( valid2 )
{
hdop = std::sqrt(covM2(0,0)+covM2(1,1));
vdop = std::sqrt(covM2(2,2));
}
else
{
hdop = -1.0;
vdop = -1.0;
}
return gData;
}
catch(Exception& u)
{
// Throw an exception if something unexpected happens
ProcessingException e( getClassName() + ":"
+ u.what() );
GPSTK_THROW(e);
}
} // End of method 'ComputeDOP::Process()'
