YSRESULT YsClipLineSegByTopSide(YsVec2 &c1,YsVec2 &c2,double topy)
{
double cut;
// if(c1.y()>topy && c2.y()>topy) // Modified 2000/12/07
if(c1.y()>topy+YsTolerance && c2.y()>topy+YsTolerance) // Modified 2000/12/07
{
return YSERR;
}
else if(YsAbs(c1.y()-c2.y())<YsTolerance)
{
return YSOK; // Note : One of them are below the top side and another is at the same height.
}
// else if(c1.y()>topy || c2.y()>topy) // Modified 2000/12/07
else if(c1.y()>topy+YsTolerance || c2.y()>topy+YsTolerance) // Modified 2000/12/07
{
cut=c1.x()+((c2.x()-c1.x())/(c2.y()-c1.y()))*(topy-c1.y());
// if(c1.y()>topy) // Modified 2000/12/07
if(c1.y()>topy+YsTolerance) // Modified 2000/12/07
{
c1.Set(cut,topy);
}
// else if(c2.y()>topy) // Modified 2000/12/07
else if(c2.y()>topy+YsTolerance)
{
c2.Set(cut,topy);
}
return YSOK;
}
return YSOK;
}
YSRESULT YsClipLineSegByBottomSide(YsVec2 &c1,YsVec2 &c2,double btmy)
{
double cut;
// if(c1.y()<btmy && c2.y()<btmy) // Modified 2000/12/07
if(c1.y()<btmy-YsTolerance && c2.y()<btmy-YsTolerance) // Modified 2000/12/07
{
return YSERR;
}
else if(YsAbs(c1.y()-c2.y())<YsTolerance)
{
return YSOK; // Note : One of them are above the bottom side and another is at the same height.
}
// else if(c1.y()<btmy || c2.y()<btmy) // Modified 2000/12/07
else if(c1.y()<btmy-YsTolerance || c2.y()<btmy-YsTolerance) // Modified 2000/12/07
{
cut=c1.x()+((c2.x()-c1.x())/(c2.y()-c1.y()))*(btmy-c1.y());
// if(c1.y()<btmy) // Modified 2000/12/07
if(c1.y()<btmy-YsTolerance) // Modified 2000/12/07
{
c1.Set(cut,btmy);
}
// else if(c2.y()<btmy) // Modified 2000/12/07
else if(c2.y()<btmy-YsTolerance) // Modified 2000/12/07
{
c2.Set(cut,btmy);
}
return YSOK;
}
return YSOK;
}
YSRESULT YsClipLineSegByRightSide(YsVec2 &c1,YsVec2 &c2,double ritx)
{
double cut;
// if(c1.x()>ritx && c2.x()>ritx) // Modified 2000/12/07
if(c1.x()>ritx+YsTolerance && c2.x()>ritx+YsTolerance) // Modified 2000/12/07
{
return YSERR;
}
else if(YsAbs(c1.x()-c2.x())<YsTolerance)
{
return YSOK;
}
// else if(c1.x()>ritx || c2.x()>ritx) // Modified 2000/12/07
else if(c1.x()>ritx+YsTolerance || c2.x()>ritx+YsTolerance) // Modified 2000/12/07
{
cut=c1.y()+((c2.y()-c1.y())/(c2.x()-c1.x()))*(ritx-c1.x());
// if(c1.x()>ritx) // Modified 2000/12/07
if(c1.x()>ritx+YsTolerance) // Modified 2000/12/07
{
c1.Set(ritx,cut);
}
// if(c2.x()>ritx) // Modified 2000/12/07
if(c2.x()>ritx+YsTolerance) // Modified 2000/12/07
{
c2.Set(ritx,cut);
}
return YSOK;
}
return YSOK;
}
YSRESULT YsMatrix::Invert(void)
{
YsMatrix sync;
sync.Create(nRow,nColumn);
sync.LoadIdentity();
int n,r;
double a;
for(n=1; n<=nColumn; n++)
{
// Sweep Out Column n
for(r=n+1; r<=nRow; r++)
{
if(YsAbs(v(n,n))<YsAbs(v(r,n)))
{
SwapRow(r,n);
sync.SwapRow(r,n);
}
}
a=v(n,n);
if(YsAbs(a)>=YsTolerance)
{
DivRow(n,a);
sync.DivRow(n,a);
for(r=1; r<=nRow; r++)
{
if(r!=n)
{
a=v(r,n);
Row1MinusRow2Mul(r,n,a);
sync.Row1MinusRow2Mul(r,n,a);
}
}
}
else
{
return YSERR;
}
}
*this=sync;
return YSOK;
}
static YSRESULT YsClipInfinite2DLineByY(YsVec2 &itsc,const YsVec2 &org,const YsVec2 &vec,const double &y)
{
if(YsAbs(vec.y())>YsTolerance)
{
double x,slope;
slope=vec.x()/vec.y();
x=org.x()+slope*(y-org.y());
itsc.Set(x,y);
return YSOK;
}
else
{
return YSERR;
}
}
YSRESULT YsLUDecomposition(YsMatrix &l,YsMatrix &u,const YsMatrix &from)
{
int r,c;
if(from.nr()==from.nc())
{
u=from;
l.Create(from.nr(),from.nc());
for(c=1; c<=u.nc(); c++)
{
l.Set(c,c,1.0);
}
for(c=1; c<u.nc(); c++) // (not c<=u.nc() because right bottom end element doesn't have to be eliminated)
{
if(YsAbs(u.v(c,c))>YsTolerance)
{
for(r=c+1; r<=u.nr(); r++)
{
double coffee;
coffee=u.v(r,c)/u.v(c,c);
l.Set(r,c,coffee);
u.Row1MinusRow2Mul(r,c,coffee);
}
}
else
{
YsErrOut("YsLUDecomposition()\n");
YsErrOut(" Cannot find the decomposition.\n");
YsErrOut(" Original Matrix's (%d,%d)=0.0\n",c,c);
return YSERR;
}
}
return YSOK;
}
else
{
YsErrOut("YsLUDecomposition()\n");
YsErrOut(" The original matrix is not a square matrix.\n");
return YSERR;
}
}
YSRESULT YsFindEigenValueEigenVectorByJacobiMethod
(YsMatrix &value,YsMatrix &vector,const YsMatrix &from,const double &tolerance)
{
int p,q,n;
YSBOOL done;
double cs,sn;
if(from.nr()==from.nc())
{
n=from.nr();
value=from;
vector.Create(value.nr(),value.nc());
vector.LoadIdentity();
done=YSFALSE;
while(done!=YSTRUE)
{
done=YSTRUE;
for(p=1; p<=n-1; p++)
{
for(q=p+1; q<=n; q++)
{
if(YsAbs(value.v(p,q))>tolerance)
{
done=YSFALSE;
YsJacobiFindAngle(cs,sn,value.v(p,p),value.v(q,q),value.v(p,q));
YsJacobiRotate(value,vector,p,q,cs,sn);
}
}
}
}
return YSOK;
}
else
{
YsErrOut("YsFindEigenValueEigenVectorByJacobiMethod()\n");
YsErrOut(" The matrix must be square, symmetric.\n");
return YSERR;
}
}
static YSRESULT YsClip3DLine
(YsVec3 &clip,const YsVec3 &org,const YsVec3 &vec,const double &clipPoint,int component)
{
int c1,c2;
double buf[3],slope[3];
if(component==0)
{
c1=1;
c2=2;
}
else if(component==1)
{
c1=0;
c2=2;
}
else
{
c1=0;
c2=1;
}
if(YsAbs(vec.GetValue()[component])>=YsTolerance)
{
slope[c1]=vec.GetValue()[c1]/vec.GetValue()[component];
slope[c2]=vec.GetValue()[c2]/vec.GetValue()[component];
buf[component]=clipPoint;
buf[c1]=org.GetValue()[c1]+slope[c1]*(clipPoint-org.GetValue()[component]);
buf[c2]=org.GetValue()[c2]+slope[c2]*(clipPoint-org.GetValue()[component]);
clip.Set(buf[0],buf[1],buf[2]);
return YSOK;
}
else
{
return YSERR;
}
}
YSRESULT YsInvertMatrix2x2(double result[4],const double from[4])
{
double *inv,buf[4],tfm[4];
const int i11=0,i12=1,i21=2,i22=3;
if(result!=from)
{
inv=result;
}
else
{
inv=buf;
}
tfm[i11]=from[i11];
tfm[i12]=from[i12];
tfm[i21]=from[i21];
tfm[i22]=from[i22];
inv[i11]=1.0;
inv[i12]=0.0;
inv[i21]=0.0;
inv[i22]=1.0;
if(YsAbs(from[i11])<YsAbs(from[i21]))
{
YsSwapDouble(tfm[i11],tfm[i21]);
YsSwapDouble(tfm[i12],tfm[i22]);
YsSwapDouble(inv[i11],inv[i21]);
YsSwapDouble(inv[i12],inv[i22]);
}
if(YsAbs(tfm[i11])<YsTolerance)
{
return YSERR;
}
double c;
c=tfm[i21]/tfm[i11];
tfm[i21] =0.0;
tfm[i22]-=tfm[i12]*c;
inv[i21]-=inv[i11]*c;
inv[i22]-=inv[i12]*c;
c=tfm[i11];
// tfm[i11]=1.0; // (1,1) is alreadydone.
tfm[i12]/=c;
inv[i11]/=c;
inv[i12]/=c;
if(YsAbs(tfm[i22])<YsTolerance)
{
return YSERR;
}
c=tfm[i12]/tfm[i22];
// tfm[i11]-=tfm[i21]*c; (Theoretically, but don't need)
// tfm[i12]-=tfm[i22]*c; (Theoretically, but don't need)
inv[i11]-=inv[i21]*c;
inv[i12]-=inv[i22]*c;
c=tfm[i22];
inv[i21]/=c;
inv[i22]/=c;
if(inv!=result)
{
result[0]=inv[0];
result[1]=inv[1];
result[2]=inv[2];
result[3]=inv[3];
}
return YSOK;
}
