// ---------------------------------------------------------------------------------
// 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);
}
}
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;
}
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;
}
/* 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);
}
