void GameStateManager::deserializePlayer(int32_t eid)
{
  PlayerState & player = *m_states[eid];

  const std::string name = m_connection_manager.getNickname(eid);
  const std::string nickhash = sha1::calcToString(name.data(), name.length());
  std::copy(nickhash.begin(), nickhash.end(), player.nickhash.begin());

  if (PROGRAM_OPTIONS.count("verbose")) 
  {
    std::cout << "Player nickname is \"" << name << "\", hash: 0x" << hexify(nickhash)
              << ". Attempting to load player data from disk...";
  }

  std::ifstream playerfile("/tmp/mymap.player." + nickhash, std::ios::binary);

  if (!playerfile)
  {
    if (PROGRAM_OPTIONS.count("verbose"))
    {
      std::cout << " failed. Setting default start data." << std::endl;
    }

    const WorldCoords start_pos(8, 80, 8);

    player.position = RealCoords(wX(start_pos) + 0.5, wY(start_pos) + 0.5, wZ(start_pos) + 0.5);
    player.stance   = wY(start_pos) + 0.5;

    player.setInv(37, ITEM_DiamondPickaxe, 1, 0);
    player.setInv(36, BLOCK_Torch, 50, 0);
    player.setInv(29, ITEM_Coal, 50, 0);
    player.setInv(21, BLOCK_Cobblestone, 60, 0);
    player.setInv(22, BLOCK_IronOre, 60, 0);
    player.setInv(30, BLOCK_Wood, 50, 0);
    player.setInv(38, ITEM_DiamondShovel, 1, 0);
    player.setInv(39, BLOCK_BrickBlock, 64, 0);
    player.setInv(40, BLOCK_Stone, 64, 0);
    player.setInv(41, BLOCK_Glass, 64, 0);
    player.setInv(42, BLOCK_WoodenPlank, 64, 0);
    player.setInv(44, BLOCK_Obsidian, 64, 0);
    player.setInv(43, ITEM_Bucket, 1, 0);

    player.holding = 4;
  }
  else
  {
    if (PROGRAM_OPTIONS.count("verbose"))
    {
      std::cout << " done!" << std::endl;
    }

    const WorldCoords start_pos(8, 80, 8);

    player.position = RealCoords(wX(start_pos) + 0.5, wY(start_pos) + 0.5, wZ(start_pos) + 0.5);
    player.stance   = wY(start_pos) + 0.5;
  }
    
}
	void Manifold::SetContactData(const glm::vec3& c, const glm::vec3& n, const float p)
	{
		contact = c;
		normal = n;

		// calculate the basis matrix for this contact
		glm::vec3 cX, cY, cZ;
		glm::vec3 wY(0, 1, 0);
		glm::vec3 wX(1, 0, 0);
		glm::mat3 contactBasis;
		cX = normal;
		if (glm::dot(normal, wY) > 0.01f)
		{
			cZ = glm::cross(cX, wY);
		}
		else
		{
			cZ = glm::cross(cX, wX);
		}
		cY = glm::cross(cZ, cX);

		contactBasis[0] = cX;
		contactBasis[1] = cY;
		contactBasis[2] = cZ;
	}
//! [Main function]
int main()
//! [Main function]
{
  //! [Create data structures]
  int npoints = 4;

  std::vector< vpColVector > x1(npoints);
  std::vector< vpColVector > x2(npoints);

  std::vector< vpColVector > wX(npoints);  // 3D points in the world plane
  std::vector< vpColVector > c1X(npoints); // 3D points in the camera frame 1
  std::vector< vpColVector > c2X(npoints); // 3D points in the camera frame 2

  for (int i = 0; i < npoints; i++) {
    x1[i].resize(3);
    x2[i].resize(3);

    wX[i].resize(4);
    c1X[i].resize(4);
    c2X[i].resize(4);
  }
  //! [Create data structures]

  //! [Simulation]
  // Ground truth pose used to generate the data
  vpHomogeneousMatrix c1Tw_truth(-0.1, 0.1, 1.2, vpMath::rad(5), vpMath::rad(0), vpMath::rad(45));
  vpHomogeneousMatrix c2Tc1(0.01, 0.01, 0.2, vpMath::rad(0), vpMath::rad(3), vpMath::rad(5));

  // Input data: 3D coordinates of at least 4 coplanar points
  double L = 0.2;
  wX[0][0] =   -L; wX[0][1] = -L; wX[0][2] = 0; wX[0][3] = 1; // wX_0 ( -L, -L, 0, 1)^T
  wX[1][0] =  2*L; wX[1][1] = -L; wX[1][2] = 0; wX[1][3] = 1; // wX_1 (-2L, -L, 0, 1)^T
  wX[2][0] =    L; wX[2][1] =  L; wX[2][2] = 0; wX[2][3] = 1; // wX_2 (  L,  L, 0, 1)^T
  wX[3][0] =   -L; wX[3][1] =  L; wX[3][2] = 0; wX[3][3] = 1; // wX_3 ( -L,  L, 0, 1)^T

  // Input data: 2D coordinates of the points on the image plane
  for(int i = 0; i < npoints; i++) {
    c1X[i] = c1Tw_truth * wX[i];          // Update c1X, c1Y, c1Z
    x1[i][0] = c1X[i][0] / c1X[i][2];     // x1 = c1X/c1Z
    x1[i][1] = c1X[i][1] / c1X[i][2];     // y1 = c1Y/c1Z
    x1[i][2] = 1;

    c2X[i] = c2Tc1 * c1Tw_truth * wX[i];  // Update cX, cY, cZ
    x2[i][0] = c2X[i][0] / c2X[i][2];     // x2 = c1X/c1Z
    x2[i][1] = c2X[i][1] / c2X[i][2];     // y2 = c1Y/c1Z
    x2[i][2] = 1;
  }
  //! [Simulation]

  //! [Call function]
  vpMatrix _2H1 = homography_dlt(x1, x2);
  //! [Call function]

  std::cout << "2H1 (computed with DLT):\n" << _2H1 << std::endl;

  return 0;
}
Exemple #4
0
Embedding MNF(const Matrix &data, const Matrix &noise_covariance, Index reduced_dimensions) {

    if (noise_covariance.cols() != noise_covariance.rows() || noise_covariance.rows() != data.cols()) {
        throw std::invalid_argument("The rows and columns of noise covariance should both equal to the columns of the data.");
    }
    if (reduced_dimensions == 0) reduced_dimensions = data.cols();
    if (reduced_dimensions > data.cols()) throw std::invalid_argument("Reduced dimensions should be less than or equal to the total dimensions.");


    Matrix U1(noise_covariance);
    Matrix V1(data.cols(), data.cols());

    gsl_vector *S1v = gsl_vector_alloc(data.cols());

    gsl_linalg_SV_decomp_jacobi(U1.m_, V1.m_, S1v);

    Matrix wX(data.rows(), data.cols());
    Matrix wXintermediate(data.cols(), data.cols());
    Matrix invsqrtS1(data.cols(), data.cols());
    for (Index i = 0; i < data.cols(); ++i) {
        invsqrtS1(i, i) = 1.0 / sqrt(gsl_vector_get(S1v, i));
    }
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, U1.m_, invsqrtS1.m_, 0.0, wXintermediate.m_);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, data.m_, wXintermediate.m_, 0.0, wX.m_);
    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, wX.m_, wX.m_, 0.0, wXintermediate.m_);


    Matrix V2(data.cols(), data.cols());
    gsl_vector *S2v = gsl_vector_alloc(data.cols());
    gsl_linalg_SV_decomp_jacobi(wXintermediate.m_, V2.m_, S2v);


    Embedding result;
    result.space = std::make_shared<gsl::Matrix>(data.rows(), reduced_dimensions);
    result.vectors = std::make_shared<gsl::Matrix>(data.cols(), data.cols());
    result.values = std::make_shared<gsl::Matrix>(1, data.cols());

    Matrix intermediate2(data.cols(), data.cols());
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, invsqrtS1.m_, V2.m_, 0.0, intermediate2.m_);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, U1.m_, intermediate2.m_, 0.0, result.vectors->m_);

    gsl_matrix_const_view reduced_vectors = gsl_matrix_const_submatrix(result.vectors->m_, 0, 0, result.vectors->rows(), reduced_dimensions);

    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, data.m_, &reduced_vectors.matrix, 0.0, result.space->m_);


    // TODO: assign eigenvalues and eigenvectors

    gsl_vector_free(S1v);
    gsl_vector_free(S2v);

    return result;
}