// 1. Calculate max radius and then max displacement for each edge.
// 2. tmin=minimum of the maximum displacements.
// 3. Calculate radius from the tmin for each edge.
YSRESULT YsShellExt_RoundUtil3d::SetUpRoundVertex(const YsShell &shl,YSSIZE_T nVt,const YsShellVertexHandle vtHdArray[])
{
const YsShellSearchTable *search=shl.GetSearchTable();
if(NULL==search)
{
YsPrintf("%s\n",__FUNCTION__);
YsPrintf(" This function requires a search table.\n");
return YSERR;
}
CleanUp();
for(YSSIZE_T vtIdx=0; vtIdx<nVt; ++vtIdx)
{
YsArray <YsShellVertexHandle> connVtHdArray;
if(YSOK==search->GetConnectedVertexList(connVtHdArray,shl,vtHdArray[vtIdx]))
{
for(auto connVtHd : connVtHdArray)
{
cornerArray.Increment();
cornerArray.Last().Initialize();
cornerArray.Last().fromVtHd=vtHdArray[vtIdx];
cornerArray.Last().toVtHd=connVtHd;
cornerArray.Last().toPos=shl.GetVertexPosition(connVtHd);
}
}
}
if(YSOK!=CalculateRoundingDirectionAll(shl))
{
CleanUp();
return YSERR;
}
return YSOK;
}
YsVec3 YsShellExt_RoundUtil3d::CalculateRoundingDirction(const YsShell &shl,YsShellVertexHandle fromVtHd,YSSIZE_T nTo,const YsShellVertexHandle toVtHd[]) const
{
const YsVec3 fromPos=shl.GetVertexPosition(fromVtHd);
YsVec3 vecSum=YsOrigin();
for(YSSIZE_T idx=0; idx<nTo; ++idx)
{
const YsVec3 vec=YsUnitVector(shl.GetVertexPosition(toVtHd[idx])-fromPos);
vecSum+=vec;
}
vecSum.Normalize();
return vecSum;
}
void PrepareShell(YsShell &shl)
{
int idx[8];
idx[0]=shl.AddVertex(YsVec3(-2.5, 1.0, 1.0));
idx[1]=shl.AddVertex(YsVec3( 2.5, 1.0, 1.0));
idx[2]=shl.AddVertex(YsVec3(-2.5, 1.0,-1.0));
idx[3]=shl.AddVertex(YsVec3( 2.5, 1.0,-1.0));
idx[4]=shl.AddVertex(YsVec3(-2.5,-1.0,-1.0));
idx[5]=shl.AddVertex(YsVec3( 2.5,-1.0,-1.0));
idx[6]=shl.AddVertex(YsVec3(-2.5,-1.0, 1.0));
idx[7]=shl.AddVertex(YsVec3( 2.5,-1.0, 1.0));
int plg[4];
plg[0]=idx[0];
plg[1]=idx[1];
plg[2]=idx[3];
plg[3]=idx[2];
shl.AddPolygon(4,plg);
plg[0]=idx[2];
plg[1]=idx[3];
plg[2]=idx[5];
plg[3]=idx[4];
shl.AddPolygon(4,plg);
plg[0]=idx[4];
plg[1]=idx[5];
plg[2]=idx[7];
plg[3]=idx[6];
shl.AddPolygon(4,plg);
plg[0]=idx[6];
plg[1]=idx[7];
plg[2]=idx[1];
plg[3]=idx[0];
shl.AddPolygon(4,plg);
plg[0]=idx[0];
plg[1]=idx[2];
plg[2]=idx[4];
plg[3]=idx[6];
shl.AddPolygon(4,plg);
plg[0]=idx[7];
plg[1]=idx[5];
plg[2]=idx[3];
plg[3]=idx[1];
shl.AddPolygon(4,plg);
}
const double YsShellExt_RoundUtil3d::CalculateMaximumRadius(const YsShell &shl) const
{
if(0==cornerArray.GetN())
{
printf("%s %d\n",__FUNCTION__,__LINE__);
return 0.0;
}
double maxRadius=YsInfinity;
for(auto corner : cornerArray)
{
if(YSTRUE==corner.notReallyDistanceLimited)
{
continue;
}
YsVec3 toVec=corner.toPos-shl.GetVertexPosition(corner.fromVtHd);
const double L=toVec.GetLength();
if(YsTolerance>L)
{
printf("%s %d\n",__FUNCTION__,__LINE__);
return 0.0;
}
toVec/=L;
YsVec3 cenVec=corner.roundDir;
if(YSOK!=cenVec.Normalize() || YsTolerance>cenVec*toVec)
{
printf("%s\n",cenVec.Txt());
printf("%s\n",toVec.Txt());
printf("%lf\n",cenVec*toVec);
printf("%s %d\n",__FUNCTION__,__LINE__);
return 0.0;
}
const double theata=acos(YsBound(toVec*cenVec,-1.0,1.0));
const double maxRadiusForCorner=L*tan(theata);
if(maxRadiusForCorner<maxRadius)
{
printf("Max radius limited by %s\n",shl.GetVertexPosition(corner.fromVtHd).Txt());
maxRadius=maxRadiusForCorner;
}
}
return maxRadius;
}
const double YsShellExt_RoundUtil::CalculateMaximumRadius(const YsShell &shl) const
{
double maxRadTotal=0.0;
for(YSSIZE_T idx=0; idx<cornerArray.GetN(); ++idx)
{
const RoundCorner &corner=cornerArray[idx];
const double edgeLen[2]=
{
shl.GetEdgeLength(corner.fromVtHd,corner.toVtHd[0]),
shl.GetEdgeLength(corner.fromVtHd,corner.toVtHd[1])
};
const double maxTanDist=YsSmaller(edgeLen[0],edgeLen[1]);
const double maxRad=CalculateRadiusFromTangentDistance(shl,corner.fromVtHd,corner.toVtHd,maxTanDist);
if(YsTolerance>maxRad)
{
return 0.0;
}
if(0==idx || maxRad<maxRadTotal)
{
maxRadTotal=maxRad;
}
}
return maxRadTotal;
}
YSRESULT YsShellExt_RoundUtil3d::CalculateRoundingAll(const YsShell &shl,const double radius)
{
YsArray <unsigned int> vtKeyArray;
YsArray <HalfRoundCorner *> cornerPtrArray;
for(auto &corner : cornerArray)
{
vtKeyArray.Append(shl.GetSearchKey(corner.fromVtHd));
cornerPtrArray.Append(&corner);
}
YsQuickSort <unsigned int,HalfRoundCorner *> (vtKeyArray.GetN(),vtKeyArray,cornerPtrArray);
YsArray <HalfRoundCorner *> cornerPerVertex;
for(YSSIZE_T idx=0; idx<cornerPtrArray.GetN(); ++idx)
{
cornerPerVertex.Append(cornerPtrArray[idx]);
if(cornerPtrArray.GetN()-1==idx || vtKeyArray[idx]!=vtKeyArray[idx+1])
{
if(YSOK!=CalculateRoundingPerVertex(shl,cornerPerVertex,radius))
{
return YSERR;
}
cornerPerVertex.CleanUp();
}
}
return YSOK;
}
void MoveShell(YsShell &sh,YsVec3 &mov)
{
YsMatrix4x4 mat,trn,viw,iViw;
sh.GetMatrix(mat);
viw=eyeAtt.GetMatrix();
iViw=viw;
iViw.Invert();
trn.Initialize();
trn.Translate(mov);
mat=viw*trn*iViw*mat;
sh.SetMatrix(mat);
}
void RotateShell(YsShell &sh,double pitch,double yaw)
{
YsMatrix4x4 mat,rot,viw,iViw;
sh.GetMatrix(mat);
viw=eyeAtt.GetMatrix();
iViw=viw;
iViw.Invert();
rot.Initialize();
rot.RotateZY(pitch);
rot.RotateXZ(yaw);
mat=viw*rot*iViw*mat;
sh.SetMatrix(mat);
}
void YsBoundingBoxMaker3::Make(const YsShell &shl,YSSIZE_T nVt,const YsShellVertexHandle vtHd[])
{
int i;
YsVec3 pos;
Begin();
for(i=0; i<nVt; i++)
{
shl.GetVertexPosition(pos,vtHd[i]);
Add(pos);
}
}
YsVec3 YsShellExt_RoundUtil3d::CalculateRoundingDirction(const YsShell &shl,const YsArray <HalfRoundCorner *> &roundCornerPerVertex) const
{
const YsVec3 fromPos=shl.GetVertexPosition(roundCornerPerVertex[0]->fromVtHd);
YsVec3 vecSum=YsOrigin();
for(auto corner : roundCornerPerVertex)
{
const YsVec3 vec=YsUnitVector(corner->toPos-fromPos);
vecSum+=vec;
}
vecSum.Normalize();
return vecSum;
}
// 1. Calculate max radius and then max displacement for each edge.
// 2. tmin=minimum of the maximum displacements.
// 3. Calculate radius from the tmin for each edge.
YSRESULT YsShellExt_RoundUtil3d::CalculateRoundingPerVertex(const YsShell &shl,const YsArray <YsShellExt_RoundUtil3d::HalfRoundCorner *> &roundCornerPerVertex,const double radius) const
{
const YsVec3 m=CalculateRoundingDirction(shl,roundCornerPerVertex);
const YsVec3 fromPos=shl.GetVertexPosition(roundCornerPerVertex[0]->fromVtHd);
double tMin=-1.0;
for(auto corner : roundCornerPerVertex)
{
const YsVec3 toPos=corner->toPos;
const double L=(toPos-fromPos).GetLength();
const double r=(YSTRUE==corner->notReallyDistanceLimited ? radius : YsSmaller(radius,CalculateRadiusFromLimit(fromPos,toPos,m,L)));
const double t=CalculateDisplacementFromRadius(fromPos,toPos,m,r);
if(-0.1>tMin || t<tMin)
{
tMin=t;
}
}
if(YsTolerance>tMin)
{
return YSERR;
}
const YsVec3 roundedCornerPos=fromPos+m*tMin;
for(auto corner : roundCornerPerVertex)
{
const YsVec3 toPos=corner->toPos;
const double r=CalculateRadiusFromDisplacement(fromPos,toPos,m,tMin);
if(YsTolerance>r)
{
return YSERR;
}
YsVec3 center;
if(YSOK!=CalculateCenterFromRadius(center,fromPos,toPos,m,r))
{
return YSERR;
}
corner->roundedCornerPos=roundedCornerPos;
YsGetNearestPointOnLine3(corner->foot,fromPos,toPos,center);
corner->radius=r;
corner->center=center;
}
return YSOK;
}
YSRESULT YsShellExt_RoundUtil::SetUpRoundPolygon(const YsShell &shl,YsShellPolygonHandle plHd,const YsShellVertexStore *roundVtx)
{
YsArray <YsShellVertexHandle> plVtHd;
shl.GetPolygon(plVtHd,plHd);
for(YSSIZE_T idx=0; idx<plVtHd.GetN(); ++idx)
{
const YsShellVertexHandle fromVtHd=plVtHd[idx];
if(NULL==roundVtx || YSTRUE==roundVtx->IsIncluded(fromVtHd))
{
const YsShellVertexHandle toVtHd[2]=
{
plVtHd.GetCyclic(idx-1),
plVtHd.GetCyclic(idx+1)
};
AddRoundCorner(shl,fromVtHd,toVtHd);
}
}
targetPlKeyArray.Append(shl.GetSearchKey(plHd));
return YSOK;
}
/*static*/ double YsShellExt_RoundUtil::CalculateRadiusFromTangentDistance(const YsShell &shl,YsShellVertexHandle fromVtHd,const YsShellVertexHandle toVtHd[2],const double dist)
{
const double edLen[2]=
{
(shl.GetVertexPosition(toVtHd[0])-shl.GetVertexPosition(fromVtHd)).GetLength(),
(shl.GetVertexPosition(toVtHd[1])-shl.GetVertexPosition(fromVtHd)).GetLength()
};
const YsVec3 edVec[2]=
{
(shl.GetVertexPosition(toVtHd[0])-shl.GetVertexPosition(fromVtHd))/edLen[0],
(shl.GetVertexPosition(toVtHd[1])-shl.GetVertexPosition(fromVtHd))/edLen[1]
};
const double theata=YsPi-acos(YsBound(edVec[0]*edVec[1],-1.0,1.0));
const double t=tan(theata/2.0);
if(YsTolerance>t)
{
return 0.0;
}
return dist/t;
}
YsArray <const YsShellExt_RoundUtil3d::HalfRoundCorner *> YsShellExt_RoundUtil3d::MakeSortedHalfRoundCorner(const YsShell &shl) const
{
YsArray <unsigned int> vtKeyArray;
YsArray <const HalfRoundCorner *> cornerPtrArray;
for(auto &corner : cornerArray)
{
vtKeyArray.Append(shl.GetSearchKey(corner.fromVtHd));
cornerPtrArray.Append(&corner);
}
YsQuickSort <unsigned int,const HalfRoundCorner *> (vtKeyArray.GetN(),vtKeyArray,cornerPtrArray);
return cornerPtrArray;
}
YSRESULT YsShellExt_PatchBetweenTwoConstEdge::SetVertexSequence(const YsShell &srcShl,
YSSIZE_T nVt0,const YsShellVertexHandle vtHdArray0[],YSBOOL isLoop0,
YSSIZE_T nVt1,const YsShellVertexHandle vtHdArray1[],YSBOOL isLoop1)
{
if((YSTRUE!=isLoop0 && 2>nVt0) || (YSTRUE==isLoop0 && 3>nVt0) ||
(YSTRUE!=isLoop1 && 2>nVt1) || (YSTRUE==isLoop1 && 3>nVt1))
{
return YSERR;
}
YsArray <YsShellVertexHandle> newVtHdArray[2];
shl.CleanUp();
isLoop[0]=isLoop0;
isLoop[1]=isLoop1;
for(int seq=0; seq<2; ++seq)
{
const YSSIZE_T nVt=(0==seq ? nVt0 : nVt1);
const YsShellVertexHandle *vtHdArray=(0==seq ? vtHdArray0 : vtHdArray1);
const YSBOOL isLoop=(0==seq ? isLoop0 : isLoop1);
totalLen[seq]=0.0;
for(YSSIZE_T idx=0; idx<nVt; ++idx)
{
auto vtHd=shl.AddVertex(srcShl.GetVertexPosition(vtHdArray[idx]));
patchVtKeyToSrcVtHd.AddElement(shl.GetSearchKey(vtHd),vtHdArray[idx]);
patchVtKeyToSeqId.AddElement(shl.GetSearchKey(vtHd),seq);
rawVtHdSeq[seq].Append(vtHd);
if(0<idx)
{
totalLen[seq]+=shl.GetEdgeLength(rawVtHdSeq[seq][idx-1],vtHd);
}
}
if(YSTRUE==isLoop && 3<=rawVtHdSeq[seq].GetN())
{
totalLen[seq]+=shl.GetEdgeLength(rawVtHdSeq[seq][0],rawVtHdSeq[seq].Last());
}
vtHdSeq[seq]=rawVtHdSeq[seq];
seqCeHd[seq]=shl.AddConstEdge(rawVtHdSeq[seq],isLoop);
}
return YSOK;
}
YSBOOL YsShellExt_RoundUtil3d::IsEdgeUsing(const YsShellVertexHandle edVtHd0,const YsShellVertexHandle edVtHd1,const YsShell &shl,const YsShellPolygonStore &polygonStore) const
{
const YsShellSearchTable *search=shl.GetSearchTable();
int nEdPl;
const YsShellPolygonHandle *edPlHd;
if(YSOK==search->FindPolygonListByEdge(nEdPl,edPlHd,shl,edVtHd0,edVtHd1))
{
for(int plIdx=0; plIdx<nEdPl; ++plIdx)
{
if(YSTRUE==polygonStore.IsIncluded(edPlHd[plIdx]))
{
return YSTRUE;
}
}
}
return YSFALSE;
}
YsArray <YSSIZE_T> YsShellExt_RoundUtil3d::FindHalfRoundCornerFromPolygon(const YsShell &shl,YsShellPolygonHandle plHd) const
{
YsArray <YsShellVertexHandle,4> plVtHd;
shl.GetPolygon(plVtHd,plHd);
YsArray <YSSIZE_T> idxArray;
for(YSSIZE_T i=0; i<plVtHd.GetN(); ++i)
{
for(YSSIZE_T j=0; j<cornerArray.GetN(); ++j)
{
if(plVtHd[i]==cornerArray[j].fromVtHd && YSTRUE==plVtHd.IsIncluded(cornerArray[j].toVtHd))
{
idxArray.Append(j);
}
}
}
return idxArray;
}
YSRESULT YsShellExt_RoundUtil::CalculateRoundingAll(const YsShell &shl,const double radius,int nDiv)
{
if(1>nDiv)
{
nDiv=1;
}
for(auto &corner : cornerArray)
{
corner.subDiv[0].CleanUp();
corner.subDiv[1].CleanUp();
const YsVec3 vtPos[3]=
{
shl.GetVertexPosition(corner.fromVtHd),
shl.GetVertexPosition(corner.toVtHd[0]),
shl.GetVertexPosition(corner.toVtHd[1])
};
const double edLen[2]=
{
(shl.GetVertexPosition(corner.toVtHd[0])-shl.GetVertexPosition(corner.fromVtHd)).GetLength(),
(shl.GetVertexPosition(corner.toVtHd[1])-shl.GetVertexPosition(corner.fromVtHd)).GetLength()
};
const YsVec3 edVec[2]=
{
(shl.GetVertexPosition(corner.toVtHd[0])-shl.GetVertexPosition(corner.fromVtHd))/edLen[0],
(shl.GetVertexPosition(corner.toVtHd[1])-shl.GetVertexPosition(corner.fromVtHd))/edLen[1]
};
const double theata=YsPi-acos(YsBound(edVec[0]*edVec[1],-1.0,1.0));
const double L=radius*tan(theata/2.0);
if(angleThr>theata)
{
return YSERR;
}
YsPlane pln[3];
if(YSOK!=pln[0].MakePlaneFromTriangle(vtPos[0],vtPos[1],vtPos[2]))
{
return YSERR;
}
pln[1].Set(vtPos[0]+edVec[0]*L,edVec[0]);
pln[2].Set(vtPos[0]+edVec[1]*L,edVec[1]);
// Haven't I written three-plane intersection?
// Plane equation:
// (p-o)*n=0
// pn-on=0
// nx*px+ny*py+nz*pz=on
{
YsMatrix3x3 mat;
YsVec3 rhs;
for(int i=0; i<3; ++i)
{
rhs[i]=pln[i].GetNormal()*pln[i].GetOrigin();
for(int j=0; j<3; ++j)
{
mat.Set(i+1,j+1,pln[i].GetNormal()[j]);
}
}
if(YSOK!=mat.Invert())
{
YsPrintf("Cannot calculate the center.\n");
return YSERR;
}
corner.center=mat*rhs;
}
corner.roundedCornerPos=YsUnitVector(vtPos[0]-corner.center);
corner.roundedCornerPos=corner.center+corner.roundedCornerPos*radius;
for(int edIdx=0; edIdx<2; ++edIdx)
{
for(int i=0; i<nDiv; ++i)
{
corner.subDiv[edIdx].Increment();
corner.subDiv[edIdx].Last().vtHd=NULL;
if(0==i)
{
corner.subDiv[edIdx].Last().pos=vtPos[0]+edVec[edIdx]*L;;
}
if(0<i)
{
const double t=1.0-(double)i/(double)nDiv;
YsVec3 pos=vtPos[0]+edVec[edIdx]*L*t;
pos-=corner.center;
pos.Normalize();
pos*=radius;
pos+=corner.center;
corner.subDiv[edIdx].Last().pos=pos;
}
}
}
}
return YSOK;
}
YsArray <YsShellExt_RoundUtil::VertexPositionPair> YsShellExt_RoundUtil::MakeRoundedVertexSequence(const YsShell &shl,YSSIZE_T nVt,const YsShellVertexHandle vtHdArrayIn[],YSBOOL isLoop) const
{
YsArray <YsShellVertexHandle> orgVtHdArray(nVt,vtHdArrayIn);
YsArray <VertexPositionPair> newVtHdArray;
for(YSSIZE_T orgVtIdx=0; orgVtIdx<orgVtHdArray.GetN(); ++orgVtIdx)
{
YSBOOL rounded=YSFALSE;
for(const auto &roundCorner : cornerArray)
{
if(roundCorner.fromVtHd==orgVtHdArray[orgVtIdx])
{
int forward=0,backward=1;
if(roundCorner.toVtHd[0]==orgVtHdArray.GetCyclic(orgVtIdx-1))
{
forward=1;
backward=0;
}
else if(roundCorner.toVtHd[1]==orgVtHdArray.GetCyclic(orgVtIdx-1))
{
forward=0;
backward=1;
}
else
{
continue;
}
YSBOOL skipFirst=YSFALSE;
if(0<newVtHdArray.GetN() && newVtHdArray.Last().pos==roundCorner.subDiv[backward].Last().pos)
{
skipFirst=YSTRUE;
}
newVtHdArray.Append(roundCorner.subDiv[backward]);
newVtHdArray.Increment();
newVtHdArray.Last().vtHd=orgVtHdArray[orgVtIdx];
newVtHdArray.Last().pos=roundCorner.roundedCornerPos;
for(YSSIZE_T i=roundCorner.subDiv[forward].GetN()-1; 0<=i; --i)
{
if(YSTRUE==skipFirst)
{
skipFirst=YSFALSE;
continue;
}
newVtHdArray.Append(roundCorner.subDiv[forward][i]);
}
rounded=YSTRUE;
}
}
if(YSTRUE!=rounded)
{
newVtHdArray.Increment();
newVtHdArray.Last().vtHd=orgVtHdArray[orgVtIdx];
newVtHdArray.Last().pos=shl.GetVertexPosition(newVtHdArray.Last().vtHd);
}
}
if(2<=newVtHdArray.GetN() && YSTRUE==isLoop && newVtHdArray[0].pos==newVtHdArray.Last().pos)
{
newVtHdArray.DeleteLast();
}
return newVtHdArray;
}
void DrawShell(YsShell &shell,YsColor &col,YSBOOL inPolygon)
{
int i,j,k;
int nVtx,nPlg;
YsVec3 vtx[256];
if(inPolygon==YSTRUE)
{
glEnable(GL_LIGHTING);
}
else
{
glDisable(GL_LIGHTING);
}
nPlg=shell.GetNumPolygon();
for(i=0; i<nPlg; i++)
{
nVtx=shell.GetNumVertexOfPolygon(i);
if(nVtx>0)
{
double r,g,b;
YsVec3 nom;
shell.GetVertexListOfPolygon(vtx,256,i);
nom=(vtx[1]-vtx[0])^(vtx[2]-vtx[1]);
nom.Normalize();
col.GetDoubleRGB(r,g,b);
glColor3d(r,g,b);
if(inPolygon==YSFALSE || YsCheckConvex3(nVtx,vtx)==YSTRUE)
{
switch(inPolygon)
{
case YSFALSE:
glBegin(GL_LINE_LOOP);
break;
case YSTRUE:
glBegin(GL_POLYGON);
break;
}
glNormal3d(nom.x(),nom.y(),nom.z());
for(j=0; j<nVtx; j++)
{
glVertex3d(vtx[j].x(),vtx[j].y(),vtx[j].z());
}
glEnd();
}
else
{
YsSword swd;
swd.SetInitialPolygon(nVtx,vtx);
swd.Convexnize();
for(j=0; j<swd.GetNumPolygon(); j++)
{
nVtx=swd.GetNumVertexOfPolygon(j);
swd.GetVertexListOfPolygon(vtx,256,j);
glBegin(GL_POLYGON);
for(k=0; k<nVtx; k++)
{
glVertex3dv(vtx[k].GetValue());
}
glEnd();
}
}
}
}
}
