示例#1
0
// ---------------------------------------------------------------------------------
// calculate bounding box
void 
Mesh3D::
calculateBoundingBox( Vector3 & bbMin, Vector3 & bbMax )
{

	if (!m_vertexPositions.empty())
	{
		bbMin = m_vertexPositions[0];
		bbMax = m_vertexPositions[0];

		std::vector<Vector3>::const_iterator  p_it(m_vertexPositions.begin());
		std::vector<Vector3>::const_iterator  p_end(m_vertexPositions.end());

		for (; p_it!=p_end; ++p_it)
		{
			if( (*p_it).x < bbMin.x ) bbMin.x = (*p_it).x;
			else if ( (*p_it).x > bbMax.x ) bbMax.x = (*p_it).x;

			if( (*p_it).y < bbMin.y ) bbMin.y = (*p_it).y;
			else if ( (*p_it).y > bbMax.y ) bbMax.y = (*p_it).y;

			if( (*p_it).z < bbMin.z ) bbMin.z = (*p_it).z;
			else if ( (*p_it).z > bbMax.z ) bbMax.z = (*p_it).z;
		}
	}
	else {
		printf("No vertex positions to calculate bounding box!\n");
		assert(false);
	}
}
示例#2
0
bool ArmControlCartesian::setTargetAngular(double x, double y, double z, double r, double p, double w, double time)
{
  if( !isEmpty() )
    return false;


  //
  // set command time
  //
  setCmdTime(m_assignedPartIndex);


  //
  // position
  //
  hrp::Vector3 p_start(m_jointPath->endLink()->p);
  hrp::Vector3 p_end(x,y,z);

  if( !m_pos_interpolator->calcInterpolation(&p_start(0), &p_end(0), time) ) {
    std::cout << m_instanceName << " : calc pos interpolation error" << std::endl;
    return false;
  }


  //
  // attitude
  //
  hrp::Matrix33 R_start(m_jointPath->endLink()->attitude());
  hrp::Matrix33 R_end(hrp::rotFromRpy(r,p,w));

  if( !m_rot_interpolator->calcInterpolation(R_start, R_end, hrp::Vector3(0,0,-1), time) ) {
    m_pos_interpolator->clear();
    std::cout << m_instanceName << " : calc rot interpolation error" << std::endl;
    return false;
  }

  //std::cout <<  m_instanceName << " : reference attitude = " << R_end << std::endl;

  return true;
}
示例#3
0
float GPS_Filter::getDistanceHeading(const GPS_Info &info)
{
	LONG prj_x, prj_y;
	LONG last_x, last_y;
	GlobalCrt crt;
	crt.geoToPrj(info.longitude, info.latitude, &prj_x, &prj_y);
	crt.geoToPrj(m_last_used.longitude, m_last_used.latitude, &last_x, &last_y);
	Pos2DEx p_start(last_x, last_y);
	Pos2DEx p_end(prj_x, prj_y);
	double angle = SpatialMath::calLineNAngle(p_start, p_end);
	float heading = angle / PI * 180.0;
	return heading;
}
示例#4
0
/* create a biarc for a bezier curve.
 *
 * extends the tangent lines to the bezier curve at its first and last control
 * points, and intersects them to find a third point.
 * the biarc passes through the first and last control points, and the incenter
 * of the circle defined by the first, last and intersection points.
 */
HIDDEN ON_Arc
make_biarc(const ON_BezierCurve& bezier)
{
    ON_2dPoint isect, arc_pt;
    ON_2dPoint p_start(bezier.PointAt(0)), p_end(bezier.PointAt(1.0));
    ON_2dVector t_start(bezier.TangentAt(0)), t_end(bezier.TangentAt(1.0));
    ON_Ray r_start(p_start, t_start), r_end(p_end, t_end);

    r_start.IntersectRay(r_end, isect);
    arc_pt = incenter(p_start, p_end, isect);

    return ON_Arc(p_start, arc_pt, p_end);
}