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;
	}
}
Пример #7
0
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;
}