C++ Vector3f::z方法代码示例

	bool EXPORT_API ProjectPointTriangleConstraintsJB(btVector3* p, btVector3* p0, btVector3* p1, btVector3* p2, float radius, float K1, float K2, btVector3* c, btVector3* c0, btVector3* c1, btVector3* c2, btVector3 *debugOut)
	{

		//btScalar massInv = 1.0f / mass;


		Eigen::Vector3f pp(p->x(), p->y(), p->z());
		Eigen::Vector3f pA(p0->x(), p0->y(), p0->z());
		Eigen::Vector3f pB(p1->x(), p1->y(), p1->z());
		Eigen::Vector3f pC(p2->x(), p2->y(), p2->z());

		Eigen::Vector3f corrA;
		Eigen::Vector3f corrB;
		Eigen::Vector3f corrC;
		Eigen::Vector3f corr;
		Eigen::Vector3f debug;

		const bool res = PBD::PositionBasedDynamics::solveTrianglePointDistConstraint(pp, 1, pA, 1, pB, 1, pC, 1, radius, K1, K2, corr, corrA, corrB, corrC);


			if (res)
			{
				c->setValue(corr.x(), corr.y(), corr.z());
				c0->setValue(corrA.x(), corrA.y(), corrA.z());
				c1->setValue(corrB.x(), corrB.y(), corrB.z());
				c2->setValue(corrC.x(), corrC.y(), corrC.z());
				debugOut->setValue(debug.x(), debug.y(), debug.z());
		
			}

		
			return res;

	}

  void WorldDownloadManager::cropCloud(const Eigen::Vector3f & min,const Eigen::Vector3f & max,
  typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud)
{
  const uint cloud_size = cloud->size();

  std::vector<bool> valid_points(cloud_size,true);

  // check the points
  for (uint i = 0; i < cloud_size; i++)
  {
    const PointT & pt = (*cloud)[i];

    if (pt.x > max.x() || pt.y > max.y() || pt.z > max.z() ||
      pt.x < min.x() || pt.y < min.y() || pt.z < min.z())
      valid_points[i] = false;
  }

  // discard invalid points
  out_cloud->clear();
  out_cloud->reserve(cloud_size);
  uint count = 0;

  for (uint i = 0; i < cloud_size; i++)
    if (valid_points[i])
    {
      out_cloud->push_back((*cloud)[i]);
      count++;
    }
  out_cloud->resize(count);
}

void X3DConverter::startShape(const std::array<float, 12>& matrix) {

    // Finding axis/angle from matrix using Eigen for its bullet proof implementation.
    Eigen::Transform<float, 3, Eigen::Affine> t;
    t.setIdentity();
    for (unsigned int i = 0; i < 3; i++) {
        for (unsigned int j = 0; j < 4; j++) {
            t(i, j) = matrix[i+j*3];
        }
    }

    Eigen::Matrix3f rotationMatrix;
	Eigen::Matrix3f scaleMatrix;
    t.computeRotationScaling(&rotationMatrix, &scaleMatrix);
	Eigen::Quaternionf q;
	Eigen::AngleAxisf aa;
	q = rotationMatrix;
    aa = q;

	Eigen::Vector3f scale = scaleMatrix.diagonal();
	Eigen::Vector3f translation = t.translation();

    startNode(ID::Transform);
    m_writers.back()->setSFVec3f(ID::translation, translation.x(), translation.y() , translation.z());
    m_writers.back()->setSFRotation(ID::rotation, aa.axis().x(), aa.axis().y(), aa.axis().z(), aa.angle());
    m_writers.back()->setSFVec3f(ID::scale, scale.x(), scale.y(), scale.z());
    startNode(ID::Shape);
    startNode(ID::Appearance);
    startNode(ID::Material);
    m_writers.back()->setSFColor<vector<float> >(ID::diffuseColor, RVMColorHelper::color(m_materials.back()));
    endNode(ID::Material); // Material
    endNode(ID::Appearance); // Appearance

}

bool targetViewpoint(const Eigen::Vector3f& rayo,const Eigen::Vector3f& target,const Eigen::Vector3f& down,
                     Eigen::Affine3f& transf)
{
  // uz: versor pointing toward the destination
  Eigen::Vector3f uz = target - rayo;
  if (std::abs(uz.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": target point on ray origin!" << std::endl;
    return false;
  }
  uz.normalize();
  //std::cout << "uz " << uz.transpose() << ", norm " << uz .norm() << std::endl;
  // ux: versor pointing toward the ground
  Eigen::Vector3f ux = down - down.dot(uz) * uz;  
  if (std::abs(ux.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": ray to target toward ground direction!" << std::endl;
    return false;
  }
  ux.normalize();
  //std::cout << "ux " << ux.transpose() << ", norm " << ux.norm() << std::endl;
  Eigen::Vector3f uy = uz.cross(ux);
  //std::cout << "uy " << uy.transpose() << ", norm " << uy.norm() << std::endl;
  Eigen::Matrix3f rot;
  rot << ux.x(), uy.x(), uz.x(),
         ux.y(), uy.y(), uz.y(),
         ux.z(), uy.z(), uz.z();
  transf.setIdentity();
  transf.translate(rayo);
  transf.rotate(rot);
  //std::cout << __FILE__ << "\nrotation\n" << rot << "\ntranslation\n" << rayo << "\naffine\n" << transf.matrix() << std::endl;
  return true;
}

	void EXPORT_API ComputeInternalConstraintsJB(btVector3* predictedPositions, float* invMasses, bool* posLocks, Link* links, int linksCount, float Ks_prime, float mass)
	{
		for (int i = 0; i < linksCount; i++)
		{
			Link link = links[i];
			if ((link.type == -1) || (posLocks[link.idA] && posLocks[link.idB]))
				continue;

			btScalar wA = posLocks[link.idA] ? 0.0f : invMasses[link.idA];
			btScalar wB = posLocks[link.idB] ? 0.0f : invMasses[link.idB];

			btVector3 predPosA = predictedPositions[link.idA];
			btVector3 predPosB = predictedPositions[link.idB];

			Eigen::Vector3f pA(predPosA.x(), predPosA.y(), predPosA.z());
			Eigen::Vector3f pB(predPosB.x(), predPosB.y(), predPosB.z());

			Eigen::Vector3f corrA;
			Eigen::Vector3f corrB;
			const bool res = PBD::PositionBasedDynamics::solveDistanceConstraint(pA, wA, pB, wB, link.restLength, Ks_prime, Ks_prime, corrA, corrB);

			if (res)
			{
				if (wA != 0.0f)
					predictedPositions[link.idA] += btVector3(corrA.x(), corrA.y(), corrA.z());
				
				if (wB != 0.0f)
					predictedPositions[link.idB] += btVector3(corrB.x(), corrB.y(), corrB.z());
			}


		}


	}

void Kamikaze::updateEnemy(Level * currLev) {
	//todo move at ship;

	if (!dead) {
		Eigen::Vector3f shipLoc = currLev->ship->position;
		Eigen::Vector3f direction = shipLoc - position;
		
		direction.normalize();
		direction /= 1000;
		direction.z() -= currLev->ship->velocity.z();



		Eigen::Vector3f mousePos = Eigen::Vector3f(shipLoc.x(), shipLoc.y(), shipLoc.z());
		float rotateY = RADIANS_TO_DEGREES(atan((shipLoc.x() - position.x()) / (shipLoc.z() - position.z()))) - 90.0;
		float rotateX = -RADIANS_TO_DEGREES(atan((shipLoc.y() - position.y()) / (shipLoc.z() - position.z())));

		rotate.y() = rotateY;
		rotate.x() = rotateX;

		// if we're behind ship, keep going in that direction
		if (direction.z() <= 0) {
			direction[2] *= -1;
			direction[2] += 0.2;
		}
		velocity = direction;
	}
}

template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> int
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::getApproxIntersectedVoxelCentersBySegment (
    const Eigen::Vector3f& origin,
    const Eigen::Vector3f& end,
    AlignedPointTVector &voxel_center_list,
    float precision)
{
  Eigen::Vector3f direction = end - origin;
  float norm = direction.norm ();
  direction.normalize ();

  const float step_size = static_cast<const float> (resolution_) * precision;
  // Ensure we get at least one step for the first voxel.
  const int nsteps = std::max (1, static_cast<int> (norm / step_size));

  OctreeKey prev_key;

  bool bkeyDefined = false;

  // Walk along the line segment with small steps.
  for (int i = 0; i < nsteps; ++i)
  {
    Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));

    PointT octree_p;
    octree_p.x = p.x ();
    octree_p.y = p.y ();
    octree_p.z = p.z ();

    OctreeKey key;
    this->genOctreeKeyforPoint (octree_p, key);

    // Not a new key, still the same voxel.
    if ((key == prev_key) && (bkeyDefined) )
      continue;

    prev_key = key;
    bkeyDefined = true;

    PointT center;
    genLeafNodeCenterFromOctreeKey (key, center);
    voxel_center_list.push_back (center);
  }

  OctreeKey end_key;
  PointT end_p;
  end_p.x = end.x ();
  end_p.y = end.y ();
  end_p.z = end.z ();
  this->genOctreeKeyforPoint (end_p, end_key);
  if (!(end_key == prev_key))
  {
    PointT center;
    genLeafNodeCenterFromOctreeKey (end_key, center);
    voxel_center_list.push_back (center);
  }

  return (static_cast<int> (voxel_center_list.size ()));
}

Eigen::Vector3f lineNormalized(Eigen::Vector3f p0, Eigen::Vector3f p1)
{
	Eigen::Vector3f l = p0.cross(p1);
	l.x() = l.x() / l.z();
	l.y() = l.y() / l.z();
	l.z() = 1.0f;
	//return l;
	return p0.cross(p1).normalized();
}

void
View::render ( void )
{
        ::glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        ::glLoadIdentity();

        // model view.
        const Camera& camera = this->_model.getCamera();
        Eigen::Vector3f eye = camera.getEye();
        Eigen::Vector3f center = camera.getCenter();
        Eigen::Vector3f up = camera.getUpVector();
        ::gluLookAt ( eye.x(), eye.y(), eye.z(),
                      center.x(), center.y(), center.z(),
                      up.x(), up.y(), up.z() );

        //light
        Light light = this->_model.getLight();
        this->setLight ( light );


        //draw mesh

        RenderingMode mode = this->_model.getPreference().getRenderingMode() ;
        if ( mode == WIRE ) {
                ::glDisable ( GL_LIGHTING );
                ::glPolygonMode ( GL_FRONT_AND_BACK, GL_LINE );
                const Color3f fg = this->_model.getPreference().getWireColor();
                ::glColor3f ( fg.x(), fg.y(), fg.z() );
        } else if ( mode == SURFACE ) {
                ::glEnable ( GL_LIGHTING );
                const Color3f fg = this->_model.getPreference().getSurfaceColor();

                ::glPolygonMode ( GL_FRONT_AND_BACK, GL_FILL );
                GLfloat mat_ambient[4] = {fg.x(), fg.y(), fg.z(), 1.0};
                GLfloat mat_diffuse[4] = {0.8,0.8, 0.8, 1.0};
                GLfloat mat_specular[4] = {0.2, 0.2, 0.2, 1.0};
                GLfloat mat_shininess[1] = {100.0f};

                ::glMaterialfv ( GL_FRONT, GL_AMBIENT,  mat_ambient );
                ::glMaterialfv ( GL_FRONT, GL_DIFFUSE,  mat_diffuse );
                ::glMaterialfv ( GL_FRONT, GL_SPECULAR, mat_specular );
                ::glMaterialfv ( GL_FRONT, GL_SHININESS,mat_shininess );

                GLfloat mat2_ambient[4] = {1-fg.x(), 1-fg.y(), 1-fg.z(), 1.0};
                GLfloat mat2_diffuse[4] = {0.8,0.8, 0.8, 1.0};
                GLfloat mat2_specular[4] = {0.2, 0.2, 0.2, 1.0};
                GLfloat mat2_shininess[1] = {100.0f};
                ::glMaterialfv ( GL_BACK, GL_AMBIENT,  mat2_ambient );
                ::glMaterialfv ( GL_BACK, GL_DIFFUSE,  mat2_diffuse );
                ::glMaterialfv ( GL_BACK, GL_SPECULAR, mat2_specular );
                ::glMaterialfv ( GL_BACK, GL_SHININESS,mat2_shininess );

        }
        this->render_mesh();
        return;
}

int Simulator::computeObservations(Mocap_object* mo, Camera* cam,  bool show_image){


  char img_name[100];
  if (show_image){
    sprintf(img_name,"Projection_%i", cam->id);
    cvNamedWindow(img_name,1);
  }

  cam->obs.clear();

  Eigen::Affine3f c2w = cam->pose.inverse();

  for (uint i=0; i<mo->points.size(); ++i){
    if (!mo->point_valid[i]) continue;
    Eigen::Vector3f  c = mo->points[i];

    c = c2w*c;

//    ROS_INFO("pt in cam frame: %f %f %f", c.x(), c.y(), c.z());

    float x_px = c.x()/c.z()*cam->f_x+cam->c_x;
    float y_px = c.y()/c.z()*cam->f_y+cam->c_y;

    // point behind camera or not within field of view
    if (c.z() < 0 || x_px < 0 || y_px < 0 || x_px >= cam->c_width || y_px >= cam->c_height){
      continue;
    }

    cam->obs[i] = cvPoint2D32f(x_px,y_px);

  }

  if (show_image) {
    // print on image
    cvSet(img, cvScalar(0,0,0));

    for (Observations::iterator it = cam->obs.begin(); it!=cam->obs.end(); ++it){
      cvCircle(img, cvPoint(it->second.x,it->second.y),3, CV_RGB(0,255,0),2);
    }

//    for (uint i=0; i<prj.size(); ++i){
//      float x = prj[i].x; float y = prj[i].y;
//      float x_ = prj[(i+1)%prj.size()].x; float y_ = prj[(i+1)%prj.size()].y;
//      cvLine(img,cvPoint(x,y),cvPoint(x_,y_), CV_RGB(255,0,0),2);
//    }
    cvShowImage(img_name, img);
    cvWaitKey(5);
  }


  return cam->obs.size();

}

template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::randomOrthogonalAxis (
                                                                                             Eigen::Vector3f const &axis,
                                                                                             Eigen::Vector3f &rand_ortho_axis)
{
  if (!areEquals (axis.z (), 0.0f))
  {
    rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
  }
  else if (!areEquals (axis.y (), 0.0f))
  {
    rand_ortho_axis.x () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
  }
  else if (!areEquals (axis.x (), 0.0f))
  {
    rand_ortho_axis.y () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.z () = (static_cast<float> (rand ()) / static_cast<float> (RAND_MAX)) * 2.0f - 1.0f;
    rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
  }

  rand_ortho_axis.normalize ();

  // check if the computed x axis is orthogonal to the normal
  //assert(areEquals(rand_ortho_axis.dot(axis), 0.0f, 1E-6f));
}

pcl::PointCloud<pcl::PointNormal>::Ptr CloudFactory::createCube(const double size_, const Eigen::Vector3f &_center, const int _npoints)
{
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud = pcl::PointCloud<pcl::PointNormal>::Ptr(new pcl::PointCloud<pcl::PointNormal>());
	int N = sqrt(_npoints / 6);


	double minX = _center.x() - size_ * 0.5;
	double minY = _center.y() - size_ * 0.5;
	double minZ = _center.z() - size_ * 0.5;

	double maxX = _center.x() + size_ * 0.5;
	double maxY = _center.y() + size_ * 0.5;
	double maxZ = _center.z() + size_ * 0.5;

	double step = size_ / N;

	// Generate faces fixed in X axis
	for (double y = minY; y <= maxY; y += step)
	{
		for (double z = minZ; z <= maxZ; z += step)
		{
			cloud->push_back(PointFactory::createPointNormal(minX, y, z, -1, 0, 0));
			cloud->push_back(PointFactory::createPointNormal(maxX, y, z, 1, 0, 0));
		}
	}

	// Generate faces fixed in Y axis
	for (double x = minX; x <= maxX; x += step)
	{
		for (double z = minZ; z <= maxZ; z += step)
		{
			cloud->push_back(PointFactory::createPointNormal(x, minY, z, 0, -1, 0));
			cloud->push_back(PointFactory::createPointNormal(x, maxY, z, 0, 1, 0));
		}
	}

	// Generate faces fixed in Z axis
	for (double x = minX; x <= maxX; x += step)
	{
		for (double y = minY; y <= maxY; y += step)
		{
			cloud->push_back(PointFactory::createPointNormal(x, y, minZ, 0, 0, -1));
			cloud->push_back(PointFactory::createPointNormal(x, y, maxZ, 0, 0, 1));
		}
	}

	return cloud;
}

void WorldDownloadManager::transformBoundingBoxAndExpand(const Eigen::Vector3f& bbox_min,const Eigen::Vector3f& bbox_max,
  const Eigen::Affine3f& transform,Eigen::Vector3f& bbox_min_out,Eigen::Vector3f& bbox_max_out)
{
  const int SIZE = 2;
  Eigen::Vector3f inv[SIZE];
  inv[0] = bbox_min;
  inv[1] = bbox_max;
  Eigen::Vector3f comb;
  for (uint ix = 0; ix < SIZE; ix++)
  {
    comb.x() = inv[ix].x();
    for (uint iy = 0; iy < SIZE; iy++)
    {
      comb.y() = inv[iy].y();
      for (uint iz = 0; iz < SIZE; iz++)
      {
        comb.z() = inv[iz].z();
        const Eigen::Vector3f t_comb = transform * comb;
        if (!ix && !iy && !iz) // first iteration
          bbox_min_out = bbox_max_out = t_comb;
        else
        {
          bbox_min_out = bbox_min_out.array().min(t_comb.array());
          bbox_max_out = bbox_max_out.array().max(t_comb.array());
        }
      }
    }
  }
}

bool CommandSubscriber::getForcedTfFrame(Eigen::Affine3f & result) const
  {
  tf::StampedTransform transform;
  try
    {
    m_tf_listener.lookupTransform(m_forced_tf_frame_reference,m_forced_tf_frame_name,ros::Time(0),transform);
    }
    catch (tf::TransformException ex)
      {
      ROS_ERROR("kinfu: hint was forced by TF, but retrieval failed because: %s",ex.what());
      return false;
      }

  Eigen::Vector3f vec;
  vec.x() = transform.getOrigin().x();
  vec.y() = transform.getOrigin().y();
  vec.z() = transform.getOrigin().z();
  Eigen::Quaternionf quat;
  quat.x() = transform.getRotation().x();
  quat.y() = transform.getRotation().y();
  quat.z() = transform.getRotation().z();
  quat.w() = transform.getRotation().w();

  result.linear() = Eigen::AngleAxisf(quat).matrix();
  result.translation() = vec;
  return true;
  }

static bool math_eigen_test_rotation_method_consistency(const Eigen::Quaternionf &q, const Eigen::Vector3f &v)
{
	bool success = true;

	assert_eigen_quaternion_is_normalized(q);

	// This is the same as computing the rotation by computing: q^-1*[0,v]*q, but cheaper
	Eigen::Vector3f v_rotated = eigen_vector3f_clockwise_rotate(q, v);

	// Make sure doing the matrix based rotation performs the same result
	{
		Eigen::Matrix3f m = eigen_quaternion_to_clockwise_matrix3f(q);
		Eigen::Vector3f v_test = m * v;

		success &= v_test.isApprox(v_rotated, k_normal_epsilon);
		assert(success);
	}

	// Make sure the Hamilton product style rotation matches
	{
		Eigen::Quaternionf v_as_quaternion = Eigen::Quaternionf(0.f, v.x(), v.y(), v.z());
		Eigen::Quaternionf q_inv = q.conjugate();
		Eigen::Quaternionf qinv_v = q_inv * v_as_quaternion;
		Eigen::Quaternionf qinv_v_q = qinv_v * q;

		success &=
			is_nearly_equal(qinv_v_q.w(), 0.f, k_normal_epsilon) &&
			is_nearly_equal(qinv_v_q.x(), v_rotated.x(), k_normal_epsilon) &&
			is_nearly_equal(qinv_v_q.y(), v_rotated.y(), k_normal_epsilon) &&
			is_nearly_equal(qinv_v_q.z(), v_rotated.z(), k_normal_epsilon);
		assert(success);
	}

	return success;
}