YSRESULT YsShellLattice::SetDomain_Polygon(const class YsShell &,YSSIZE_T nPl,const YsShellPolygonHandle plHdList[])
{
int i;
YsShellPolygonHandle plHd;
YsLoopCounter ctr;
YSRESULT res;
plHd=NULL;
res=YSOK;
ctr.BeginCounter(nPl);
for(i=0; i<nPl; i++)
{
ctr.Increment();
if(YSOK==res)
{
if(AddPolygon(plHdList[i])!=YSOK)
{
YsErrOut("YsShellLattice::SetDomain()\n");
YsErrOut(" Cannot add polygon\n");
res=YSERR;
}
}
}
ctr.EndCounter();
return res;
}
// Note : If from is non-symmetric or non-positive-definite,
// the result will be wrong. This should be checked by an application.
YSRESULT YsCholeskyDecomposition(YsMatrix &l,const YsMatrix &from)
{
YsMatrix L,U,D;
if(YsLUDecomposition(L,U,from)==YSOK)
{
int r;
D.Create(from.nr(),from.nc());
for(r=1; r<=from.nr(); r++)
{
D.Set(r,r,sqrt(U.v(r,r)));
}
l=L*D;
return YSOK;
}
else
{
YsErrOut("YsCholeskyDecomposition()\n");
YsErrOut(" Cannot find LUDecomposition.\n");
return YSERR;
}
}
const YsMatrix &YsMatrix::operator+=(const YsMatrix &a)
{
if(this!=&a)
{
if(nr()==a.nr() && nc()==a.nc())
{
int r,c;
for(r=1; r<=nr(); r++)
{
for(c=1; c<=nc(); c++)
{
Set(r,c,v(r,c)+a.v(r,c));
}
}
return *this;
}
else
{
YsErrOut("YsMatirx::operator+=\n Number of rows and/or columns doesn't match.\n");
return *this;
}
}
else
{
int r,c;
for(r=1; r<=nr(); r++)
{
for(c=1; c<=nc(); c++)
{
Set(r,c,v(r,c)*2);
}
}
return *this;
}
}
YsMatrix *YsMatrix::UseTemporaryMatrix(void) const
{
if(tmp!=NULL)
{
return tmp; // Direct Recycle. Good for environment?
}
else
{
tmp=new YsMatrix;
if(tmp==NULL)
{
YsErrOut("YsMatrix::UseTemporaryMatrix(void)\n");
YsErrOut("Low memory warning!!!\n");
}
return tmp;
}
}
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;
}
}
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;
}
}
YSRESULT YsMatrix::Transpose(void)
{
if(nRow!=nColumn)
{
YsErrOut("YsMatrix::Transpose() doesn't suppose non-square matrix currently.\n");
return YSERR;
}
else
{
int r,c;
double a,b;
for(r=1; r<=nRow; r++)
{
for(c=r; c<=nColumn; c++)
{
a=v(r,c);
b=v(c,r);
Set(r,c,b);
Set(c,r,a);
}
}
return YSOK;
}
}
YSRESULT YsShell::MergeMsh(const char fn[])
{
FILE *fp;
int nNod,nEl2;
YsShellVertexHandle *nodVtHd;
int ac;
char buf[256],str[256],*av[32];
nNod=0;
nEl2=0;
nodVtHd=NULL;
fp=fopen(fn,"r");
if(fp!=NULL)
{
while(fgets(str,256,fp)!=NULL)
{
int i;
for(i=0; str[i]!=0; i++)
{
if(str[i]=='#')
{
str[i]=0;
break;
}
}
strcpy(buf,str);
YsArguments(&ac,av,32,buf);
if(ac>0)
{
if(strcmp(av[0],"nofnod")==0)
{
int i;
nNod=atoi(av[1]);
nodVtHd=new YsShellVertexHandle [nNod+1];
if(nodVtHd==NULL)
{
YsExceptionHandler::Exception(YsExceptionHandler::OUTOFMEMORY,"YsShell::MergeMsh()");
}
for(i=0; i<nNod; i++)
{
nodVtHd[i]=NULL;
}
}
else if(strcmp(av[0],"nofel2")==0)
{
nEl2=atoi(av[1]);
}
else if(strcmp(av[0],"nod")==0)
{
if(nodVtHd!=NULL)
{
int ndId;
YsVec3 pos;
ndId=atoi(av[1]);
pos.Set(atof(av[2]),atof(av[3]),atof(av[4]));
nodVtHd[ndId]=AddVertexH(pos);
}
else
{
YsErrOut("YsShell::LoadTri()\n");
YsErrOut(" nofnod didn't come before nod\n");
fclose(fp);
goto ERREND;
}
}
else if(strcmp(av[0],"el2")==0)
{
YsShellVertexHandle vtHd[4];
if(atoi(av[2])==3)
{
vtHd[0]=nodVtHd[atoi(av[3])];
vtHd[1]=nodVtHd[atoi(av[4])];
vtHd[2]=nodVtHd[atoi(av[5])];
if(vtHd[0]!=NULL && vtHd[1]!=NULL && vtHd[2]!=NULL)
{
AddPolygonH(3,vtHd);
}
}
else if(atoi(av[2])==4)
{
vtHd[0]=nodVtHd[atoi(av[3])];
vtHd[1]=nodVtHd[atoi(av[4])];
vtHd[2]=nodVtHd[atoi(av[5])];
vtHd[3]=nodVtHd[atoi(av[6])];
if(vtHd[0]!=NULL && vtHd[1]!=NULL && vtHd[2]!=NULL && vtHd[3]!=NULL)
{
AddPolygonH(4,vtHd);
}
}
else
{
YsErrOut("YsShell::LoadMsh()\n");
YsErrOut(" Load Error\n");
YsErrOut(" Refered to undefined node or non-tri&non-quad element\n");
fclose(fp);
goto ERREND;
}
}
}
}
}
fclose(fp);
return YSOK;
ERREND:
if(nodVtHd!=NULL)
{
delete [] nodVtHd;
}
return YSERR;
}
YSRESULT YsShell::LoadTri(const char fn[])
{
FILE *fp;
int nNod,nTri;
YsShellVertexHandle *nodVtHd;
int ac;
char buf[256],str[256],*av[32];
nNod=0;
nTri=0;
nodVtHd=NULL;
fp=fopen(fn,"r");
if(fp!=NULL)
{
while(fgets(str,256,fp)!=NULL)
{
strcpy(buf,str);
YsArguments(&ac,av,32,buf);
if(ac>0)
{
if(strcmp(av[0],"nofnod")==0)
{
int i;
nNod=atoi(av[1]);
nodVtHd=new YsShellVertexHandle [nNod+1];
if(nodVtHd==NULL)
{
YsExceptionHandler::Exception(YsExceptionHandler::OUTOFMEMORY,"YsShell::LoadTri()");
}
for(i=0; i<nNod; i++)
{
nodVtHd[i]=NULL;
}
}
else if(strcmp(av[0],"noftri")==0)
{
nTri=atoi(av[1]);
}
else if(strcmp(av[0],"nod")==0)
{
if(nodVtHd!=NULL)
{
int ndId;
YsVec3 pos;
ndId=atoi(av[1]);
pos.Set(atof(av[2]),atof(av[3]),atof(av[4]));
nodVtHd[ndId]=AddVertexH(pos);
}
else
{
YsErrOut("YsShell::LoadTri()\n");
YsErrOut(" nofnod didn't come before nod\n");
fclose(fp);
goto ERREND;
}
}
else if(strcmp(av[0],"tri")==0)
{
YsShellVertexHandle vtHd[3];
vtHd[0]=nodVtHd[atoi(av[2])];
vtHd[1]=nodVtHd[atoi(av[3])];
vtHd[2]=nodVtHd[atoi(av[4])];
if(vtHd[0]!=NULL && vtHd[1]!=NULL && vtHd[2]!=NULL)
{
AddPolygonH(3,vtHd);
}
else
{
YsErrOut("YsShell::LoadTri()\n");
YsErrOut(" Nodes must be defined before triangles.\n");
fclose(fp);
goto ERREND;
}
}
}
}
}
fclose(fp);
return YSOK;
ERREND:
if(nodVtHd!=NULL)
{
delete [] nodVtHd;
}
return YSERR;
}