Example #1
0
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();
}
Example #2
0
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)];
}
Example #3
0
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);
}
Example #4
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);
}
Example #5
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);
}
Example #6
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);
}
Example #7
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;
}