/*
Calculate the line segment PaPb that is the shortest route between
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
Pa = P1 + mua (P2 - P1)
Pb = P3 + mub (P4 - P3)
Return FALSE if no solution exists.
*/
Segment3D get_shortest_segment(const Segment3D &sa,
const Segment3D &sb) {
const double eps= .0001;
algebra::Vector3D va= sa.get_point(1) - sa.get_point(0);
algebra::Vector3D vb= sb.get_point(1) - sb.get_point(0);
double ma= va*va;
double mb= vb*vb;
// if one of them is too short to have a well defined direction
// just look at an endpoint
if (ma < eps && mb < eps) {
return Segment3D(sa.get_point(0), sb.get_point(0));
} else if (ma < eps) {
return get_reversed(get_shortest_segment(sb, sa.get_point(0)));
} else if (mb < eps) {
return get_shortest_segment(sa, sb.get_point(0));
}
algebra::Vector3D vfirst = sa.get_point(0)- sb.get_point(0);
IMP_LOG_VERBOSE( vfirst << " | " << va << " | " << vb << std::endl);
double dab = vb*va;
double denom = ma * mb - dab * dab;
// they are parallel or anti-parallel
if (std::abs(denom) < eps) {
return get_shortest_segment_parallel(sa, sb);
}
double dfb = vfirst*vb;
double dfa = vfirst*va;
double numer = dfb * dab - dfa * mb;
double fa = numer / denom;
double fb = (dfb + dab * fa) / mb;
/*
denom = d2121 * d4343 - d4321 * d4321;
numer = d1343 * d4321 - d1321 * d4343;
*mua = numer / denom;
*mub = (d1343 + d4321 * (*mua)) / d4343;
pa->x = p1.x + *mua * p21.x;
pa->y = p1.y + *mua * p21.y;
pa->z = p1.z + *mua * p21.z;
pb->x = p3.x + *mub * p43.x;
pb->y = p3.y + *mub * p43.y;
pb->z = p3.z + *mub * p43.z;
*/
algebra::Vector3D ra=get_clipped_point(sa, fa);
algebra::Vector3D rb=get_clipped_point(sb, fb);
IMP_LOG_VERBOSE( fa << " " << fb << std::endl);
return Segment3D(ra, rb);
}
bool Segment3D::is_point_projection_in_segment(Point3D pt_3d) {
Vector3D pt_vector = Segment3D( start_pt(), pt_3d ).as_vector();
double scal_prod = as_vector().scalar_prod( pt_vector );
double segm_length = length();
if (0 <= scal_prod and scal_prod <= segm_length*segm_length) {
return true; }
else {
return false; };
};
/*bool FaceSetPart::checkCollisionWith(SolidEntity &solid)
{
if(checkBoundingBoxes(solid)==false)return false;
//antes de proceder a la detección concreta... procedo a ver
//pared y bounding box
BoundingBox bb=solid.getAbsoluteBoundingBox();
for(int i=0;i<(int)absolutefaces.size();i++){
if(bb.checkMinMax(absolutefaces[i]))
{
//detección concreta y exacta face contra solid
//tendría que detectar si es un basic solid en cuyo caso
//solicito el modelo en alambre y detecto colisión contra él
return true;
}
}
return false;
}*/
void FaceSetPart::createWiredModel()
{
int i,j,num;
wiredModel.clear();
for(i=0;i<(int)faces.size();i++){
num=faces[i].getNumVertex();
for(j=0;j<num;j++){
wiredModel.push_back(Segment3D(faces[i].getAbsoluteVertex(j),faces[i].getAbsoluteVertex((j+1)%num)));
}
}
setWiredModelNeedToBeUpdated();
}
// Determines if 3D triangles intersect.
// If parallel, returns false. (This may be considered misleading.)
// If true and rpoint is not NULL, returns two edge/triangle intersections.
int Intersects(const Triangle3D& tri1, const Triangle3D& tri2, std::pair<Point3D, Point3D> *rpoints)
{
// check if coplanar
if ( abs( ( ( tri1[1] - tri1[0] ) ^ ( tri1[2] - tri1[1] ) ) * ( tri2[1] - tri2[0] ) ) > epsilon )
return 0;
std::vector<Point3D> points;
// test first tri's edges against second tri
for ( unsigned i = 0; i < 3; ++i )
{
unsigned j = ( i == 2 ) ? 0 : i + 1;
Point3D isect;
if ( Intersects( Segment3D( tri1[i], tri1[j] ), tri2, &isect ) )
points.push_back( isect );
}
// test second tri's edges against first tri
for ( unsigned i = 0; i < 3; ++i )
{
unsigned j = ( i == 2 ) ? 0 : i + 1;
Point3D isect;
if ( Intersects( Segment3D( tri2[i], tri2[j] ), tri1, &isect ) )
points.push_back( isect );
}
if ( rpoints && !points.empty() )
{
rpoints->first = points[0];
rpoints->second = points[1];
}
return points.size();
}
//metric used to evaluate de distances: it could be any measure, but
//the less the better
double WBState::distanceTo(RobotState *p)
{
WBState *naux=dynamic_cast<WBState *>(p);
if(!naux)return 10000.0;
Vector3D aux=naux->pose-pose;
double val=aux*aux;
if(2*aux.z*aux.z>val)val=2*aux.z*aux.z;
//val*=(2*PI/(2*PI+naux->getSize()+getSize()));
double h=2*robot->getWheelRadius();
Segment3D segm=Segment3D(pose+Vector3D(0,0,h),naux->pose+Vector3D(0,0,h));
robot->setIntersectable(false);
//penalties the intersection
if(world->segmentIntersection(segm,0))val*=8;
robot->setIntersectable(true);
return val;
}
Segment3D get_shortest_segment(const Segment3D &s,
const algebra::Vector3D &p) {
double f= get_relative_projection_on_segment(s, p);
return Segment3D(get_clipped_point(s, f), p);
}
