void Vector3D::normalize()
{
auto d = sqrt(scalarMul(*this));
if (d > 0.0)
{
(*this) /= d;
}
}
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;
}
void partMul(void) {
int i;
for (i = 0; i < colsCount; ++i)
presult[startCol + i] = scalarMul(pvec, pcols + matSize * i);
printf("[%d] partMul done\n", commRank);
}
