void Matrix<real>::AppendColumns(const Matrix<real> & columns)
{
if (columns.Getm() != m)
{
printf("Error: mismatch in number of rows in AppendColumns.\n");
throw 41;
}
data = (real*) realloc (data, sizeof(real) * (m * (n + columns.Getn())));
memcpy(&data[ELT(m, 0, n)], columns.GetData(), sizeof(real) * m * columns.Getn());
n += columns.Getn();
}
void Matrix<real>::SetSubmatrix(int I, int J, const Matrix<real> & submatrix)
{
int subm = submatrix.Getm();
int subn = submatrix.Getn();
real * subdata = submatrix.GetData();
if ((I < 0) || (J < 0) || (I + subm > m) || (J + subn > n))
{
printf("Error: matrix index out of bounds.\n");
throw 21;
}
for(int j=0; j<subn; j++)
for(int i=0; i<subm; i++)
data[ELT(m,I+i,J+j)] = subdata[ELT(subm,i,j)];
}
void Render::Triangle(const CGUL::Vector2& position, const CGUL::Vector2& pointA, const CGUL::Vector2& pointB, const CGUL::Vector2& pointC, const CGUL::Color& color)
{
using namespace CGUL;
if (doNotDraw)
{
return;
}
Matrix model;
model = model * Matrix::MakeTranslation(position);
GL::UniformMatrix4fv(GL::GetUniformLocation(shaderProgram, "modelMatrix"), 1, false, model.GetData());
GL::Uniform4f(GL::GetUniformLocation(shaderProgram, "color"), color);
GL::Uniform1i(GL::GetUniformLocation(shaderProgram, "shaderType"), 1);
CGUL::Vector2 positions[3] = { pointA, pointB, pointC };
GL::Uniform2fv(GL::GetUniformLocation(shaderProgram, "positions"), 3, positions);
GL::BindVertexArray(vertexArrayTriangle);
GL::DrawArrays(GL_TRIANGLES, 0, 3);
GL::BindVertexArray(0);
GL::Uniform1i(GL::GetUniformLocation(shaderProgram, "shaderType"), 0);
}
void Render::Line(const CGUL::Vector2& start, const CGUL::Vector2& end, const CGUL::Color& color)
{
using namespace CGUL;
if (doNotDraw)
{
return;
}
Matrix model = Matrix::Identity();
GL::UniformMatrix4fv(GL::GetUniformLocation(shaderProgram, "modelMatrix"), 1, false, model.GetData());
GL::Uniform4f(GL::GetUniformLocation(shaderProgram, "color"), color);
GL::Uniform1i(GL::GetUniformLocation(shaderProgram, "shaderType"), 1);
CGUL::Vector2 positions[2] = { start, end };
GL::Uniform2fv(GL::GetUniformLocation(shaderProgram, "positions"), 2, positions);
GL::BindVertexArray(vertexArrayLine);
GL::DrawArrays(GL_LINES, 0, 2);
GL::BindVertexArray(0);
GL::Uniform1i(GL::GetUniformLocation(shaderProgram, "shaderType"), 0);
}
void Render::Circle(const CGUL::Vector2& position, CGUL::Float32 radius, CGUL::Float32 orientation, const CGUL::Color& color)
{
using namespace CGUL;
if (doNotDraw)
{
return;
}
Matrix model;
model = model * Matrix::MakeScaling(Vector2(radius, radius));
model = model * Matrix::MakeRotation(orientation);
model = model * Matrix::MakeTranslation(position);
GL::UniformMatrix4fv(GL::GetUniformLocation(shaderProgram, "modelMatrix"), 1, false, model.GetData());
GL::Uniform4f(GL::GetUniformLocation(shaderProgram, "color"), color);
GL::BindVertexArray(vertexArrayCircle);
GL::DrawArrays(GL_TRIANGLE_FAN, 0, circlePrecision + 2);
GL::BindVertexArray(0);
}
void Render::Box(const CGUL::Vector2& position, const CGUL::Vector2& size, CGUL::Float32 orientation, const CGUL::Color& color)
{
using namespace CGUL;
if (doNotDraw)
{
return;
}
Matrix model;
model = model * Matrix::MakeScaling(size);
model = model * Matrix::MakeRotation(orientation);
model = model * Matrix::MakeTranslation(position);
GL::UniformMatrix4fv(GL::GetUniformLocation(shaderProgram, "modelMatrix"), 1, false, model.GetData());
GL::Uniform4f(GL::GetUniformLocation(shaderProgram, "color"), color);
GL::BindVertexArray(vertexArrayBox);
GL::DrawArrays(GL_QUADS, 0, 4);
GL::BindVertexArray(0);
}
void Render::Sprite(CGUL::UInt32 texture, const CGUL::Vector2& position, const CGUL::Vector2& size)
{
using namespace CGUL;
if (doNotDraw)
{
return;
}
Matrix model;
model = model * Matrix::MakeScaling(size);
model = model * Matrix::MakeTranslation(position);
GL::UniformMatrix4fv(GL::GetUniformLocation(shaderProgram, "modelMatrix"), 1, false, model.GetData());
GL::Uniform1i(GL::GetUniformLocation(shaderProgram, "shaderType"), 2);
GL::Uniform1i(GL::GetUniformLocation(shaderProgram, "texture"), 0);
GL::ActiveTexture(GL_TEXTURE0);
GL::BindTexture(GL_TEXTURE_2D, texture);
GL::BindVertexArray(vertexArraySprite);
GL::DrawArrays(GL_QUADS, 0, 4);
GL::Uniform1i(GL::GetUniformLocation(shaderProgram, "shaderType"), 0);
GL::BindVertexArray(0);
}
ReceiverOutput Calculate_Position::GetPosition(const long double elevation_mask, int weight_method)
{
ECEF_Frame position = ECEF_Frame(0.0L, 0.0L, 0.0L);
Matrix G;
Matrix dr;
Matrix W;
int j = 0;
long double r;
Matrix cov;
int number_of_satellites = 0;
std::vector<ECEF_Frame> satellites;
std::map<int, PsudoRange> psudodistance;
GPS_Time modifiedCurrent;
std::vector<long double> original_distance;
std::vector<long double> distance;
std::vector<long double> clockdiff;
std::vector<long double> sat_weight;
long double diff;
diff = calc_pos::max_diff;
long double receiver_clockdiff = 0.0L;
while(diff > calc_pos::min_diff)
{
int i = 0;
satellites.clear();
original_distance.clear();
clockdiff.clear();
distance.clear();
sat_weight.clear();
psudodistance.clear();
modifiedCurrent = GPS_Time(current.GetWeek(), current.GetSecond() - receiver_clockdiff / IS_GPS_200::C_velocity, current.GetLeapSecond());
std::map<int, PsudoRange>::iterator current_dist_it = originalpsudodistance.begin();
for (std::map <int, Ephemeris>::iterator it = ephemeris.begin(); it != ephemeris.end(); it++)
{
for (std::map<int, PsudoRange>::iterator dist_it = current_dist_it; dist_it != originalpsudodistance.end(); dist_it++)
{
if (dist_it->first == it->first)
{
long double r = dist_it->second.GetPsudoRange(type) - receiver_clockdiff;
long double satellite_clock = (it->second).GetClock(modifiedCurrent, r);
r = dist_it->second.GetPsudoRange(type) - receiver_clockdiff + satellite_clock * IS_GPS_200::C_velocity;
dist_it->second.SetCalculateRange(r);
satellite_clock = (it->second).GetClock(modifiedCurrent, r);
ECEF_Frame satellite_position = (it->second).GetPosition(modifiedCurrent, r);
//
if (weight_method == 1)
{
if (j < 1)
{
sat_weight.push_back(1.0L);
}
else
{
long double elevation = ENU_Frame(satellite_position, position).GetElevation();
long double sin_e = sin(elevation);
long double weight = sin_e * sin_e / calc_pos::VAR_ZENITH;
if (weight < calc_pos::MIN_WEIGHT)
{
weight = calc_pos::MIN_WEIGHT;
}
else
{
// Do nothing
}
}
}
else
{
if (j < 3 || (ENU_Frame(satellite_position, position).GetElevation() > elevation_mask))
{
sat_weight.push_back(1.0L);
}
else
{
continue;
}
}
original_distance.push_back(dist_it->second.GetPsudoRange(type));
satellites.push_back(satellite_position);
dist_it->second.SetSatellitePosition(satellite_position);
psudodistance.insert(std::pair<int, PsudoRange>(dist_it->first, dist_it->second));
clockdiff.push_back(satellite_clock);
distance.push_back(satellite_position.Distance(position));
current_dist_it = dist_it;
i++;
break;
}
else
{
continue;
}
}
}
number_of_satellites = i;
G.SetSize(number_of_satellites, 4);
dr.SetSize(number_of_satellites, 1);
W.SetSize(number_of_satellites, number_of_satellites);
for (int k = 0; k < number_of_satellites; k++)
{
for (int l = 0; l < number_of_satellites; l++)
{
W.SetData(0.0L, l, k);
}
}
// Create Observation matrix
for (i = 0; i < number_of_satellites; i++)
{
r = satellites[i].Distance(position);
ENU_Frame enu(satellites[i], position);
G.SetData(-enu.GetE()/r, i, 0);
G.SetData(-enu.GetN()/r, i, 1);
G.SetData(-enu.GetU()/r, i, 2);
G.SetData(1.0, i, 3);
dr.SetData(original_distance[i] + clockdiff[i] * IS_GPS_200::C_velocity - r - receiver_clockdiff, i, 0);
W.SetData(sat_weight[i], i, i);
if (j > 3)
{
long double atomospheric_delay = TropoSphere::GetTropoSphereCollection(satellites[i], position);
if (ionosphere.IsValid())
{
atomospheric_delay += ionosphere.GetIonoSphereDelayCorrection(satellites[i], position, current);
}
else
{
// Do nothing
}
dr.SetData(dr.GetData(i, 0) + atomospheric_delay, i, 0);
}
}
Matrix Gt = G.Tranposed();
Matrix Gtdr = Gt * dr;
Gaussian_Elimination gauss;
Matrix GtGd;
if (j == 1)
{
GtGd = Gt;
}
else
{
GtGd = Gt * W;
}
Matrix GtG = GtGd * G;
if (j == 1)
{
cov = gauss.GetAnswer(GtG, Gtdr);
}
else
{
gauss.GetAnswer(GtG, Gtdr);
}
position= ENU_Frame(Gtdr.GetData(0, 0), Gtdr.GetData(1, 0), Gtdr.GetData(2, 0), WGS84_Frame(position)).GetPosition();
receiver_clockdiff += Gtdr.GetData(3, 0);
diff = ECEF_Frame(Gtdr).Distance(ECEF_Frame(0, 0, 0));
if (j > calc_pos::max_loop)
{
break;
}
else
{
//Do nothing
}
j++;
}
ReceiverOutput ret = ReceiverOutput(modifiedCurrent, position, psudodistance, cov, type);
return ret;
}