const double YsShellExt_RoundUtil3d::CalculateRadiusFromLimit(const YsVec3 &fromPos,const YsVec3 &toPos,const YsVec3 &moveDir,const double L) const
{
const YsVec3 edgeVec=YsUnitVector(toPos-fromPos);
if(YsOrigin()==moveDir || YsOrigin()==edgeVec)
{
return 0.0;
}
const double theata=acos(edgeVec*moveDir);
return L*tan(theata);
}
YSRESULT YsShellExt_RoundUtil3d::CalculateRoundingDirectionAll(const YsShell &shl)
{
YsArray <HalfRoundCorner *> cornerPtrArray=MakeSortedHalfRoundCorner(shl);
YsArray <HalfRoundCorner *> cornerPerVertex;
for(YSSIZE_T idx=0; idx<cornerPtrArray.GetN(); ++idx)
{
cornerPerVertex.Append(cornerPtrArray[idx]);
if(cornerPtrArray.GetN()-1==idx || cornerPtrArray[idx]->fromVtHd!=cornerPtrArray[idx+1]->fromVtHd)
{
const YsVec3 roundDir=CalculateRoundingDirction(shl,cornerPerVertex);
if(YsOrigin()==roundDir)
{
return YSERR;
}
for(auto ptr : cornerPerVertex)
{
ptr->roundDir=roundDir;
}
cornerPerVertex.CleanUp();
}
}
return YSOK;
}
YsViewPoint::YsViewPoint()
{
viewTarget=YsOrigin();
viewAttitude=YsZeroAtt();
viewAttitude.SetH(YsPi);
viewDistance=0.0;
matrixCached=YSFALSE;
}
const double YsShellExt_RoundUtil3d::CalculateDisplacementFromRadius(const YsVec3 &fromPos,const YsVec3 &toPos,const YsVec3 &moveDir,const double radius) const
{
const YsVec3 edgeVec=YsUnitVector(toPos-fromPos);
if(YsOrigin()==moveDir || YsOrigin()==edgeVec)
{
return 0.0;
}
const double theata=acos(edgeVec*moveDir);
const double s=sin(theata);
if(YsTolerance>s)
{
return 0.0;
}
return (radius/s)-radius;
}
// moveDir must be a unit vector.
YSRESULT YsShellExt_RoundUtil3d::CalculateCenterFromRadius(YsVec3 ¢er,const YsVec3 &fromPos,const YsVec3 &toPos,const YsVec3 &moveDir,const double radius) const
{
const YsVec3 edgeVec=YsUnitVector(toPos-fromPos);
if(YsOrigin()==moveDir || YsOrigin()==edgeVec)
{
return YSERR;
}
const double theata=acos(edgeVec*moveDir);
const double s=sin(theata);
if(YsTolerance>s)
{
return YSERR;
}
const double t=(radius/s)-radius;
center=fromPos+moveDir*(radius+t);
return YSOK;
}
void YsShellExt_RoundUtil3d::HalfRoundCorner::Initialize(void)
{
fromVtHd=NULL;
toVtHd=NULL;
roundDir=YsOrigin();
subDiv.CleanUp();
offsetUtil=NULL;
offsetUtil_newVtxIdx=0;
notReallyDistanceLimited=YSFALSE;
}
YSRESULT YsShellExt_RoundUtil3d::HalfRoundCorner::SubdivideByNumberOfSubdivision(int nDiv)
{
const YsVec3 v1=YsUnitVector(roundedCornerPos-center);
const YsVec3 v2=YsUnitVector(foot-center);
const double fanAngle=acos(YsBound(v1*v2,-1.0,1.0));
const YsVec3 axis=YsUnitVector(v2^v1);
if(YsOrigin()==v1 || YsOrigin()==v2 || YsOrigin()==axis)
{
return YSERR;
}
const YsVec3 baseVec=foot-center;
subDiv.CleanUp();
for(int i=0; i<nDiv; ++i)
{
subDiv.Increment();
if(0==i)
{
subDiv.Last().vtHd=NULL;
subDiv.Last().pos=foot;
}
else
{
const double t=(double)i/(double)nDiv;
const double angle=fanAngle*t;
YsVec3 spokeVec;
YsRotation rot(axis,angle);
rot.RotatePositive(spokeVec,baseVec);
subDiv.Last().vtHd=NULL;
subDiv.Last().pos=spokeVec+center;
}
}
return YSOK;
}
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;
}
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;
}
const double YsShellExt_RoundUtil3d::CalculateRadiusFromDisplacement(const YsVec3 &fromPos,const YsVec3 &toPos,const YsVec3 &moveDir,const double t) const
{
// t=(r/sin)-r
// t*sin=r-r*sin
// t*sin=(1-sin)*r
// r=t*sin/(1-sin)
const YsVec3 edgeVec=YsUnitVector(toPos-fromPos);
if(YsOrigin()==moveDir || YsOrigin()==edgeVec)
{
return 0.0;
}
const double theata=acos(edgeVec*moveDir);
const double s=sin(theata);
if(YsTolerance>1.0-s)
{
return 0.0;
}
return (t*s)/(1.0-s);
}
const YsVec3 YsTraceLineSegment::GetCurrentTangent(void) const
{
YsVec3 t;
if(curPos.seg<lSeg.GetN()-1)
{
t=lSeg[curPos.seg+1]-lSeg[curPos.seg];
t.Normalize();
return t;
}
else if(2<=lSeg.GetN())
{
t=lSeg[lSeg.GetN()-1]-lSeg[lSeg.GetN()-2];
t.Normalize();
return t;
}
return YsOrigin();
}
YSRESULT YsCollisionOfPolygon::PrecomputeProjectionOfPolygon2(void)
{
if(np2>0 && p2!=NULL)
{
if(pln2.GetNormal()!=YsOrigin())
{
YsAtt3 att;
YsVec3 prj;
YsBoundingBoxMaker2 makeBbx;
int i;
att.SetForwardVector(pln2.GetNormal());
att.GetMatrix4x4(p2PrjMat);
p2PrjMat.Invert();
p2PrjMat.Translate(-pln2.GetOrigin());
p2Prj.Set(np2,NULL);
makeBbx.Begin();
for(i=0; i<np2; i++)
{
prj=p2PrjMat*p2[i];
p2Prj[i].GetXY(prj);
makeBbx.Add(p2Prj[i]);
}
makeBbx.Get(p2PrjMin,p2PrjMax);
return YSOK;
}
else
{
YsErrOut("YsCollisionOfPolygon::PrecomputeProjectionOfPolygon2()\n");
YsErrOut(" Normal of polygon2 is not set.\n");
return YSERR;
}
}
else
{
YsErrOut("YsCollisionOfPolygon::PrecomputeProjectionOfPolygon2()\n");
YsErrOut(" This function must be called after SetPolygon2()\n");
return YSERR;
}
}
void YsBoundingBoxMaker3::Begin(void)
{
min=YsOrigin();
max=YsOrigin();
first=YSTRUE;
}
