bool getPlanefromLine(XgyLine l, XgyPlane &pl)
{
XgyPlane floor;
floor.setPlane(0,0,1,0);
//计算线段两端点的3维位置
cv::Point3f wP1;
if(!(img2world(l.x1,floor,wP1)))
{
//cout<<"Can not create plane!"<<endl;
return false;
}
cv::Point3f wP2;
if(!(img2world(l.x2,floor,wP2)))
{
//cout<<"Can not create plane!"<<endl;
return false;
}
XgyVector3D tempNormal; //平面法向垂直于直线wP1wP2，且平行于z=0
tempNormal.x=wP1.y-wP2.y;
tempNormal.y=wP2.x-wP1.x;
tempNormal.z=0;
normalizeVector3D(tempNormal);
XgyVector3D view;
getcamPose(camPos);
view=setVector3D(camPos,wP1);
normalizeVector3D(view);
XgyVector3D normal;
if(dotProduct3D(tempNormal,view)>0) //若法向与视线同向
scalarMul(-1.0,tempNormal,normal); //反向
else
scalarMul(1.0,tempNormal,normal);
double d=-1*(normal.x*wP1.x+normal.y*wP1.y+normal.z*wP1.z);
pl.setPlane(normal.x,normal.y,normal.z,d);
return true;
}
// Check Compare before and after normals of affected faces.
// If a normal changes more by more than pi/2 (90 deg), then
// we will disallow this contraction and terminate early.
BOOL Pair::normalFlips(SmallPtrSet& updatedFaces, SmallPtrSet& rvFaces, BOOL& smallNormalChange)
{
Vertex* keepVertex = getContractTarget();
Vertex* removeVertex = NULL;
if (keepVertex == v1)
removeVertex = v2;
else
removeVertex = v1;
// Compute adjusted faces:
rvFaces.Clear();
updatedFaces.Clear();
removeVertex->computeFaceSet(rvFaces); // problem here, rvFaces different
SmallPtrSet_Difference(&rvFaces, &m_Faces, &updatedFaces);
Vertex *v1, *v2, *v3;
U32 SetCtx = 0;
Face* face = (Face*)updatedFaces.Begin(SetCtx);
smallNormalChange = TRUE;
F32 dotThresh = 0.966f; // about 15 degrees
F32 worstDot=1;
BOOL result = FALSE;
IV3D u, v, oldNormal, newNormal;
F32 dot;
while(face && !result)
{
if( !result )
{
v1 = face->a->getCommonVertex(face->b);
v2 = face->b->getCommonVertex(face->c);
v3 = face->c->getCommonVertex(face->a);
// Compute the normal - we could grab this data from the equation of the plane (A, B, C, D),
// but then we'd have to keep A, B, C, D around in the faces:
subtract3D ((IV3D*)&v2->v, (IV3D*)&v1->v, (IV3D*)&u);
if( isZero(&u) )
{
if(cost < NORMAL_FLIP_COST)
cost = NORMAL_FLIP_COST;
result = TRUE;
}
}
if( !result )
{
normalize3D ((IV3D*)&u);
subtract3D ((IV3D*)&v3->v, (IV3D*)&v1->v, (IV3D*)&v);
if( isZero(&v) )
{
if(cost < NORMAL_FLIP_COST)
cost = NORMAL_FLIP_COST;
result = TRUE;
}
}
if( !result )
{
normalize3D ((IV3D*)&v);
crossprod ((IV3D*)&u, (IV3D*)&v, &oldNormal);
if( isZero(&oldNormal) )
{
if(cost < NORMAL_FLIP_COST)
cost = NORMAL_FLIP_COST;
result = TRUE;
}
}
if( !result )
normalize3D (&oldNormal);
// Recompute the normal:
if( !result )
{
// Install the keep vertex into this temp version of the adjusted face:
if (v1 == removeVertex) v1 = keepVertex;
if (v2 == removeVertex) v2 = keepVertex;
if (v3 == removeVertex) v3 = keepVertex;
subtract3D ((IV3D*)&v2->v, (IV3D*)&v1->v, (IV3D*)&u);
if( isZero(&u) )
{
if(cost < NORMAL_FLIP_COST)
cost = NORMAL_FLIP_COST;
result = TRUE;
}
}
if( !result )
{
normalize3D ((IV3D*)&u);
subtract3D ((IV3D*)&v3->v, (IV3D*)&v1->v, (IV3D*)&v);
if( isZero(&v) )
{
if(cost < NORMAL_FLIP_COST)
cost = NORMAL_FLIP_COST;
result = TRUE;
}
}
if( !result )
{
normalize3D ((IV3D*)&v);
crossprod ((IV3D*)&u, (IV3D*)&v, (IV3D*)&newNormal);
if( isZero(&newNormal) )
{
if(cost < NORMAL_FLIP_COST)
cost = NORMAL_FLIP_COST;
result = TRUE;
}
}
if( !result )
{
normalize3D ((IV3D*)&newNormal);
dot = dotProduct3D ((IV3D*)&oldNormal, (IV3D*)&newNormal);
// Did the normal flip?:
if (dot < cosMaxNormalChange)
{
// Mark it flipped:
if(cost < NORMAL_FLIP_COST)
cost = NORMAL_FLIP_COST;
result = TRUE;
}
}
// test
if( !result )
{
if(dot < worstDot)
worstDot = dot;
face = (Face*)updatedFaces.Next(SetCtx);
}
}
if(worstDot < dotThresh && !result)
{
smallNormalChange = FALSE;
}
return result;
}
