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; }